$search
00001 /* 00002 * Copyright (c) 2009, Willow Garage, Inc. 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 the 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 <ros/ros.h> 00031 00032 #include "pr2_interactive_object_detection/pr2_interactive_object_detection_backend.h" 00033 #include "pr2_interactive_object_detection/GraspableObjectList.h" 00034 00035 #include <household_objects_database_msgs/GetModelDescription.h> 00036 #include <household_objects_database_msgs/GetModelMesh.h> 00037 00038 #include <sensor_msgs/Image.h> 00039 #include <sensor_msgs/CameraInfo.h> 00040 #include <sensor_msgs/PointCloud2.h> 00041 00042 #include <stereo_msgs/DisparityImage.h> 00043 00044 #include <object_segmentation_gui/ObjectSegmentationGuiAction.h> 00045 00046 #include <tabletop_object_detector/marker_generator.h> 00047 #include <tabletop_object_detector/TabletopDetection.h> 00048 00049 using namespace tabletop_object_detector; 00050 using namespace object_recognition_gui; 00051 using namespace household_objects_database_msgs; 00052 using namespace pr2_interactive_object_detection; 00053 using namespace rgbd_assembler; 00054 00055 InteractiveObjDetBackend::InteractiveObjDetBackend() : 00056 root_nh_(""), 00057 priv_nh_("~"), 00058 tf_listener_( ros::Duration(60), true ) 00059 { 00060 obj_rec_popup_client_ = new actionlib::SimpleActionClient<ObjectRecognitionGuiAction> 00061 ("object_recognition_popup", true); 00062 00063 user_command_server_ = new actionlib::SimpleActionServer<UserCommandAction> 00064 (root_nh_, "object_detection_user_command", boost::bind(&InteractiveObjDetBackend::processUserCommand, this, _1), false ); 00065 user_command_server_->start(); 00066 00067 ROS_INFO("Connecting services.."); 00068 00069 if ( !connectService<RgbdAssembly>( rgbd_assembler_client_, "rgbd_assembly" ) ) return; 00070 00071 if ( !connectService<TabletopSegmentation>( auto_seg_client_, "tabletop_segmentation" ) ) return; 00072 00073 if ( !connectService<TabletopObjectRecognition>( auto_obj_rec_client_, "tabletop_object_recognition" ) ) return; 00074 00075 if ( !connectService<TabletopDetection>( tabletop_detection_client_, "tabletop_detection" ) ) return; 00076 00077 if ( !connectService<GetModelDescription> 00078 ( get_model_description_client_, "objects_database_node/get_model_description" ) ) return; 00079 00080 if ( !connectService<GetModelMesh> 00081 ( get_model_mesh_client_, "objects_database_node/get_model_mesh" ) ) return; 00082 00083 result_publisher_ = root_nh_.advertise<GraspableObjectList>("interactive_object_recognition_result", 1, true ); 00084 00085 marker_pub_ = priv_nh_.advertise<visualization_msgs::Marker>("object_markers", 1); 00086 00087 priv_nh_.param<double>("min_marker_quality", min_marker_quality_, 0.003); 00088 priv_nh_.param<std::string>("robot_reference_frame_id", robot_reference_frame_id_, "base_link"); 00089 priv_nh_.param<double>("table_x", table_x_, 1); 00090 priv_nh_.param<double>("table_y", table_y_, 0); 00091 priv_nh_.param<double>("table_z", table_z_, 0); 00092 00093 ROS_INFO("Backend server for Interactive Object Detection GUI started"); 00094 } 00095 00096 00097 InteractiveObjDetBackend::~InteractiveObjDetBackend() 00098 { 00099 delete obj_rec_popup_client_; 00100 delete user_command_server_; 00101 } 00102 00103 00104 template <class ServiceType> 00105 bool InteractiveObjDetBackend::connectService( ros::ServiceClient& service_client, std::string topic ) 00106 { 00107 /* 00108 if ( service_client.isValid() ) 00109 return; 00110 00111 while ( !ros::service::waitForService(topic, ros::Duration(2.0)) && priv_nh_.ok() ) 00112 { 00113 ROS_INFO_STREAM("Waiting for '" << topic << "' service to come up.."); 00114 } 00115 if ( !root_nh_.ok() ) return false; 00116 */ 00117 00118 //we only need non-persistent connections here 00119 service_client = root_nh_.serviceClient<ServiceType>(topic, false); 00120 return true; 00121 } 00122 00123 00124 void InteractiveObjDetBackend::processUserCommand(const UserCommandGoal::ConstPtr &goal) 00125 { 00126 switch (goal->request) 00127 { 00128 case UserCommandGoal::SEGMENT: 00129 doSegmentation( goal->interactive ); 00130 break; 00131 case UserCommandGoal::RECOGNIZE: 00132 doRecognition( goal->interactive ); 00133 break; 00134 case UserCommandGoal::DETECT: 00135 doDetection( goal->interactive ); 00136 break; 00137 case UserCommandGoal::RESET: 00138 resetMarkers(); 00139 break; 00140 default: 00141 ROS_ERROR("Unknown user command requested."); 00142 break; 00143 } 00144 00145 if (user_command_server_->isPreemptRequested()) 00146 { 00147 preemptAction(); 00148 } 00149 } 00150 00151 00152 void InteractiveObjDetBackend::statusFeedback( std::string msg ) 00153 { 00154 UserCommandFeedback feedback; 00155 feedback.status = msg; 00156 ROS_INFO_STREAM( feedback.status ); 00157 user_command_server_->publishFeedback( feedback ); 00158 } 00159 00160 void InteractiveObjDetBackend::abortAction( std::string msg ) 00161 { 00162 ROS_ERROR_STREAM( "Action aborted: " << msg ); 00163 UserCommandResult result; 00164 user_command_server_->setAborted( result, msg ); 00165 } 00166 00167 void InteractiveObjDetBackend::finishAction( std::string msg ) 00168 { 00169 if ( user_command_server_->isPreemptRequested() ) 00170 { 00171 ROS_WARN( "Preempt requested - action cannot be finished" ); 00172 return; 00173 } 00174 ROS_INFO_STREAM( "Action finished: " << msg ); 00175 UserCommandResult result; 00176 user_command_server_->setSucceeded( result, msg ); 00177 } 00178 00179 void InteractiveObjDetBackend::preemptAction( ) 00180 { 00181 ROS_WARN_STREAM( "Action canceled." ); 00182 UserCommandResult result; 00183 user_command_server_->setPreempted( result, "Action canceled." ); 00184 } 00185 00186 bool InteractiveObjDetBackend::getSensorData( ros::Duration time_out ) 00187 { 00188 ros::Time start_time = ros::Time::now(); 00189 if (!rgbd_assembler_client_.call(rgbd_assembler_srv_)) 00190 { 00191 abortAction("Call to rgbd assembler service failed"); 00192 return false; 00193 } 00194 ROS_INFO_STREAM("Detection backend: assembled data received after " << ros::Time::now() - start_time << " seconds"); 00195 if (rgbd_assembler_srv_.response.result != rgbd_assembler_srv_.response.SUCCESS) 00196 { 00197 std::ostringstream s; 00198 s << "RGB-D Assembler service returned error " << (int)rgbd_assembler_srv_.response.result; 00199 abortAction( s.str() ); 00200 return false; 00201 } 00202 00203 image_ = rgbd_assembler_srv_.response.image; 00204 disparity_image_ = rgbd_assembler_srv_.response.disparity_image; 00205 camera_info_ = rgbd_assembler_srv_.response.camera_info; 00206 point_cloud_ = rgbd_assembler_srv_.response.point_cloud; 00207 00208 return true; 00209 } 00210 00211 bool InteractiveObjDetBackend::doInteractiveSegmentation( ) 00212 { 00213 statusFeedback("Waiting for user input .." ); 00214 00215 std::string segm_topic("segmentation_popup"); 00216 object_segmentation_gui::ObjectSegmentationGuiGoal segm_goal; 00217 actionlib::SimpleActionClient<object_segmentation_gui::ObjectSegmentationGuiAction> 00218 os_gui_action_client(segm_topic, true); 00219 00220 while(!os_gui_action_client.waitForServer(ros::Duration(2.0)) && priv_nh_.ok() 00221 && !user_command_server_->isPreemptRequested()) { 00222 ROS_INFO("Waiting for action client on topic %s", segm_topic.c_str()); 00223 } 00224 00225 if (!priv_nh_.ok()) return false; 00226 //check for cancel request 00227 if (user_command_server_->isPreemptRequested()) return false; 00228 00229 segm_goal.disparity_image = disparity_image_; 00230 segm_goal.camera_info = camera_info_; 00231 segm_goal.point_cloud = point_cloud_; 00232 00233 os_gui_action_client.sendGoal(segm_goal); 00234 ROS_INFO("Send Data as goal"); 00235 while (!os_gui_action_client.waitForResult(ros::Duration(0.5)) && priv_nh_.ok() 00236 && !user_command_server_->isPreemptRequested()) { 00237 ROS_INFO_STREAM("Waiting for result from action client on topic" << segm_topic); 00238 } 00239 00240 //make sure we cancel the gui request if we're canceled 00241 if ( user_command_server_->isPreemptRequested() ) { 00242 os_gui_action_client.cancelGoal(); 00243 return false; 00244 } 00245 00246 if (os_gui_action_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) { 00247 00248 abortAction( "The interactive segmentation has not succeeded;"); 00249 return false; 00250 } 00251 00252 object_segmentation_gui::ObjectSegmentationGuiResult segm_result = *(os_gui_action_client.getResult()); 00253 if (segm_result.result != object_segmentation_gui::ObjectSegmentationGuiResult::SUCCESS) { 00254 std::ostringstream s; 00255 s << "Interactive Segmentation returned error " << (int)segm_result.result; 00256 abortAction( s.str() ); 00257 return false; 00258 } 00259 00260 segmentation_result_.result = segm_result.result; 00261 segmentation_result_.clusters = segm_result.clusters; 00262 segmentation_result_.table = segm_result.table; 00263 00264 return true; 00265 } 00266 00267 00268 bool InteractiveObjDetBackend::doAutoSegmentation( ) 00269 { 00270 tabletop_object_detector::TabletopSegmentation segmentation_srv; 00271 00272 if (!auto_seg_client_.call(segmentation_srv)) 00273 { 00274 abortAction("Call to segmentation service failed"); 00275 return false; 00276 } 00277 00278 if (segmentation_srv.response.result != TabletopSegmentationResponse::SUCCESS) 00279 { 00280 std::ostringstream s; 00281 s << "Segmentation service returned error " << (int)segmentation_srv.response.result; 00282 abortAction( s.str() ); 00283 return false; 00284 } 00285 00286 segmentation_result_ = segmentation_srv.response; 00287 00288 return true; 00289 } 00290 00291 00292 bool InteractiveObjDetBackend::doSegmentation( bool interactive ) 00293 { 00294 if (!getSensorData(ros::Duration(10.0))) return false; 00295 00296 segmentation_was_interactive_ = interactive; 00297 recognition_result_.cluster_model_indices.clear(); 00298 recognition_result_.models.clear(); 00299 00300 if ( interactive ) 00301 { 00302 if ( !doInteractiveSegmentation() ) return false; 00303 } 00304 else 00305 { 00306 if ( !doAutoSegmentation() ) return false; 00307 } 00308 00309 //add the table to the collision map 00310 addTableToCollisionMap(segmentation_result_.table); 00311 00312 std::ostringstream s; 00313 s << "Found " << (int)segmentation_result_.clusters.size() << " clusters."; 00314 finishAction( s.str() ); 00315 00316 int n; 00317 publishResult( n ); 00318 00319 return true; 00320 } 00321 00322 00323 bool InteractiveObjDetBackend::doInteractiveRecognition() 00324 { 00325 //wait for popup server 00326 actionlib::SimpleActionClient<ObjectRecognitionGuiAction> or_gui_action_client("object_recognition_popup", true); 00327 00328 statusFeedback("Waiting for object recognition popup service.." ); 00329 00330 while(!or_gui_action_client.waitForServer(ros::Duration(2.0)) && priv_nh_.ok() && !user_command_server_->isPreemptRequested()) 00331 { 00332 ROS_INFO("Waiting for ObjectRecognitionGuiAction server"); 00333 } 00334 00335 if (!priv_nh_.ok() || user_command_server_->isPreemptRequested()) return false; 00336 00337 ObjectRecognitionGuiGoal rec_goal; 00338 rec_goal.image = image_; 00339 rec_goal.camera_info = camera_info_; 00340 00341 //get all the meshes from the database & transforms from tf 00342 00343 statusFeedback("Getting models and coordinate transforms.." ); 00344 00345 int num_models = recognition_result_.models.size(); 00346 rec_goal.model_hypotheses.resize( num_models ); 00347 00348 for ( int m=0; m<num_models; ++m ) 00349 { 00350 std::vector<household_objects_database_msgs::DatabaseModelPose> &model_pose_list = 00351 recognition_result_.models[m].model_list; 00352 00353 int num_hypotheses = model_pose_list.size(); 00354 00355 rec_goal.model_hypotheses[m].hypotheses.resize( num_hypotheses ); 00356 00357 if ( num_hypotheses > 0 ) 00358 { 00359 rec_goal.model_hypotheses[m].accept = model_pose_list[0].confidence < min_marker_quality_; 00360 } 00361 else 00362 { 00363 rec_goal.model_hypotheses[m].accept = false; 00364 } 00365 00366 for ( int h=0; h<num_hypotheses; ++h ) 00367 { 00368 arm_navigation_msgs::Shape mesh; 00369 00370 if ( !getModelMesh( model_pose_list[h].model_id, rec_goal.model_hypotheses[m].hypotheses[h].mesh ) ) 00371 { 00372 abortAction( "Failed to get model mesh." ); 00373 return false; 00374 } 00375 00376 ROS_INFO_STREAM( "Model " << m << ", hypothesis " << h << " (database id " 00377 << model_pose_list[h].model_id << ") has " << mesh.triangles.size()/3 << " triangles and " 00378 << mesh.vertices.size() << " vertices." ); 00379 00380 rec_goal.model_hypotheses[m].hypotheses[h].pose = model_pose_list[h].pose; 00381 00382 if ( !transformPose( rec_goal.model_hypotheses[m].hypotheses[h].pose, disparity_image_.header.frame_id ) ) 00383 { 00384 abortAction( "Failed to get transform." ); 00385 return false; 00386 } 00387 00388 if ( user_command_server_->isPreemptRequested() ) 00389 { 00390 return false; 00391 } 00392 } 00393 } 00394 00395 or_gui_action_client.sendGoal(rec_goal); 00396 00397 statusFeedback("Waiting for user input.." ); 00398 00399 // wait for gui action server & send request 00400 while (!or_gui_action_client.waitForResult(ros::Duration(2.0)) && priv_nh_.ok() && !user_command_server_->isPreemptRequested()) 00401 { 00402 ROS_INFO_STREAM("Waiting for ObjectRecognitionGuiAction result"); 00403 } 00404 00405 //make sure we cancel the gui request if we're being canceled 00406 if ( user_command_server_->isPreemptRequested() ) 00407 { 00408 or_gui_action_client.cancelGoal(); 00409 return false; 00410 } 00411 00412 object_recognition_gui::ObjectRecognitionGuiResultConstPtr rec_result; 00413 rec_result = or_gui_action_client.getResult(); 00414 00415 if ( num_models != (int)rec_result->selected_hypothesis_indices.size() ) 00416 { 00417 abortAction( "GUI returned invalid result (# of models in result don't match request.)" ); 00418 return false; 00419 } 00420 00421 // only keep the selected model/pose hypotheses in the recognition result 00422 00423 for ( int m=0; m<num_models; ++m ) 00424 { 00425 std::vector<household_objects_database_msgs::DatabaseModelPose> &model_pose_list = 00426 recognition_result_.models[m].model_list; 00427 00428 int sel_hyp_index = rec_result->selected_hypothesis_indices[m]; 00429 00430 if ( sel_hyp_index >=0 && sel_hyp_index < (int)model_pose_list.size() ) 00431 { 00432 household_objects_database_msgs::DatabaseModelPose sel_hyp = model_pose_list[sel_hyp_index]; 00433 sel_hyp.confidence = 0; 00434 model_pose_list.clear(); 00435 model_pose_list.push_back( sel_hyp ); 00436 } 00437 else 00438 { 00439 model_pose_list.clear(); 00440 } 00441 } 00442 00443 if (!priv_nh_.ok()) return false; 00444 00445 return true; 00446 } 00447 00448 00449 bool InteractiveObjDetBackend::doAutoRecognition( ) 00450 { 00451 statusFeedback("Doing automatic recognition." ); 00452 00453 tabletop_object_detector::TabletopObjectRecognition recognition_srv; 00454 00455 //call automatic recognition service 00456 recognition_srv.request.table = segmentation_result_.table; 00457 recognition_srv.request.clusters = segmentation_result_.clusters; 00458 recognition_srv.request.num_models = 5; 00459 recognition_srv.request.perform_fit_merge = !segmentation_was_interactive_; 00460 00461 if (!auto_obj_rec_client_.call(recognition_srv)) 00462 { 00463 abortAction("Call to recognition service failed"); 00464 return false; 00465 } 00466 00467 //no objects found -> abort 00468 if (recognition_srv.response.models.size() == 0) 00469 { 00470 abortAction( "No objects found." ); 00471 return false; 00472 } 00473 else 00474 { 00475 std::ostringstream s; 00476 s << recognition_srv.response.models.size() << " objects recognized."; 00477 statusFeedback( s.str() ); 00478 } 00479 00480 recognition_result_ = recognition_srv.response; 00481 00482 return true; 00483 } 00484 00485 bool InteractiveObjDetBackend::doRecognition( bool interactive ) 00486 { 00487 if ( segmentation_result_.clusters.empty() ) 00488 { 00489 abortAction("No clusters found or no segmentation done yet."); 00490 return false; 00491 } 00492 00493 // call recognition service 00494 if ( !doAutoRecognition() ) return false; 00495 00496 if (user_command_server_->isPreemptRequested()) return false; 00497 00498 //call gui popup service if in interactive mode 00499 if ( interactive ) 00500 { 00501 if ( !doInteractiveRecognition() ) return false; 00502 } 00503 00504 if (user_command_server_->isPreemptRequested()) return false; 00505 00506 // publish result 00507 ROS_INFO( "Object detection procedure finished. Publishing result." ); 00508 int num_recognized; 00509 00510 if ( !publishResult( num_recognized ) ) 00511 { 00512 abortAction( "An error occured while gathering the result data." ); 00513 return false; 00514 } 00515 00516 std::ostringstream s; 00517 s << num_recognized << " of " << segmentation_result_.clusters.size() << " objects recognized."; 00518 finishAction( s.str() ); 00519 00520 return true; 00521 } 00522 00523 00524 bool InteractiveObjDetBackend::doAutoDetection( ) 00525 { 00526 statusFeedback("Doing automatic detection." ); 00527 00528 tabletop_object_detector::TabletopDetection tabletop_detection_srv; 00529 00530 //call tabletop detection service 00531 tabletop_detection_srv.request.return_clusters = false; 00532 tabletop_detection_srv.request.return_models = true; 00533 tabletop_detection_srv.request.num_models = 5; 00534 00535 if (!tabletop_detection_client_.call(tabletop_detection_srv)) 00536 { 00537 abortAction("Call to tabletop detection service failed"); 00538 return false; 00539 } 00540 00541 //no objects found -> abort 00542 if (tabletop_detection_srv.response.detection.models.size() == 0) 00543 { 00544 abortAction( "No objects found." ); 00545 return false; 00546 } 00547 else 00548 { 00549 std::ostringstream s; 00550 s << tabletop_detection_srv.response.detection.models.size() << " objects recognized."; 00551 statusFeedback( s.str() ); 00552 00553 //transform poses to odom_combined frame, so they're no longer head-relative 00554 for(size_t i=0; i<tabletop_detection_srv.response.detection.models.size(); i++) 00555 { 00556 if ( !transformPose( tabletop_detection_srv.response.detection.models[i].model_list[0].pose, "/odom_combined") ) 00557 { 00558 abortAction( "Failed to get transform to odom_combined." ); 00559 return false; 00560 } 00561 } 00562 } 00563 00564 ROS_INFO( "Tabletop detection returned %d clusters.", (int)tabletop_detection_srv.response.detection.clusters.size() ); 00565 00566 segmentation_result_.clusters = tabletop_detection_srv.response.detection.clusters; 00567 recognition_result_.cluster_model_indices = tabletop_detection_srv.response.detection.cluster_model_indices; 00568 recognition_result_.models = tabletop_detection_srv.response.detection.models; 00569 00570 //tod hack 00571 // segmentation_result_.clusters.clear(); 00572 // recognition_result_.cluster_model_indices.clear(); 00573 00574 return true; 00575 } 00576 00577 00578 bool InteractiveObjDetBackend::doDetection( bool interactive ) 00579 { 00580 // call recognition service 00581 if ( !doAutoDetection() ) return false; 00582 00583 if (user_command_server_->isPreemptRequested()) return false; 00584 00585 if (!getSensorData(ros::Duration(10.0))) return false; 00586 00587 //call gui popup service if in interactive mode 00588 if ( interactive ) 00589 { 00590 if ( !doInteractiveRecognition() ) return false; 00591 } 00592 00593 if (user_command_server_->isPreemptRequested()) return false; 00594 00595 // publish result 00596 ROS_INFO( "Object detection procedure finished. Publishing result." ); 00597 int num_recognized; 00598 00599 if ( !publishResult( num_recognized ) ) 00600 { 00601 abortAction( "An error occured while gathering the result data." ); 00602 return false; 00603 } 00604 00605 std::ostringstream s; 00606 s << num_recognized << " of " << segmentation_result_.clusters.size() << " objects recognized."; 00607 finishAction( s.str() ); 00608 00609 return true; 00610 } 00611 00612 bool InteractiveObjDetBackend::publishResult( int &num_recognized ) 00613 { 00614 if ( segmentation_result_.clusters.size() == 0 && recognition_result_.models.size() == 0 ) 00615 { 00616 ROS_ERROR( "No results available for publishing!" ); 00617 return false; 00618 } 00619 00620 num_recognized = 0; 00621 GraspableObjectList grasp_obj_list; 00622 00623 std::string reference_frame_id; 00624 00625 if ( segmentation_result_.clusters.size() > 0 ) 00626 { 00627 reference_frame_id = segmentation_result_.clusters[0].header.frame_id; 00628 } 00629 else 00630 { 00631 if ( recognition_result_.models[0].model_list.size() == 0 ) 00632 { 00633 ROS_ERROR( "Model list 0 from recognition result is empty!" ); 00634 return false; 00635 } 00636 reference_frame_id = recognition_result_.models[0].model_list[0].pose.header.frame_id; 00637 } 00638 00639 ROS_INFO( "The reference frame is %s", reference_frame_id.c_str() ); 00640 00641 grasp_obj_list.camera_info = camera_info_; 00642 grasp_obj_list.image = image_; 00643 getPose( grasp_obj_list.reference_to_camera, reference_frame_id, disparity_image_.header.frame_id ); 00644 00645 unsigned num_clusters = segmentation_result_.clusters.size(); 00646 unsigned num_models = recognition_result_.models.size(); 00647 00648 std::set<unsigned> used_model_indices; 00649 std::set<unsigned> used_cluster_indices; 00650 00651 ROS_INFO( "Number of clusters: %d models: %d", num_clusters, num_models ); 00652 00653 //first, go through all clusters and see if they match any model 00654 //store the info as graspable object 00655 for ( unsigned cluster_index=0; cluster_index<num_clusters; ++cluster_index ) 00656 { 00657 object_manipulation_msgs::GraspableObject graspable_object; 00658 arm_navigation_msgs::Shape mesh; 00659 00660 //populate cluster 00661 graspable_object.cluster = segmentation_result_.clusters[cluster_index]; 00662 graspable_object.reference_frame_id = reference_frame_id; // should be robot_reference_frame_id_; 00663 00664 //if we have recognition results, populate model data 00665 if ( recognition_result_.cluster_model_indices.size() == num_clusters ) 00666 { 00667 unsigned model_index = recognition_result_.cluster_model_indices[cluster_index]; 00668 00669 //skip models that we have already processed 00670 //a better way would be to actually merge all clusters that point to the same model, 00671 //but we'll go with that for now 00672 if ( used_model_indices.find( model_index ) != used_model_indices.end() ) 00673 { 00674 continue; 00675 } 00676 00677 used_model_indices.insert( model_index ); 00678 00679 if ( model_index > recognition_result_.models.size() ) 00680 { 00681 ROS_ERROR( "Invalid model index for cluster %d!", cluster_index ); 00682 return false; 00683 } 00684 00685 graspable_object.cluster = segmentation_result_.clusters[cluster_index]; 00686 graspable_object.reference_frame_id = robot_reference_frame_id_; 00687 00688 //only get the mesh if we have recognition results and 00689 //the first one is good enough 00690 if ( recognition_result_.models[ model_index ].model_list.size() == 0 || 00691 recognition_result_.models[ model_index ].model_list[0].confidence > min_marker_quality_ ) 00692 { 00693 ROS_INFO( "Model %d was not recognized.", model_index ); 00694 00695 //get pose of cluster relative to the camera frame 00696 if ( graspable_object.cluster.header.frame_id != reference_frame_id ) 00697 { 00698 ROS_ERROR( "Point cluster has wrong frame id (%s)", graspable_object.cluster.header.frame_id.c_str() ); 00699 return false; 00700 } 00701 } 00702 else 00703 { 00704 graspable_object.potential_models = recognition_result_.models[ model_index ].model_list; 00705 00706 if ( !getModelMesh( graspable_object.potential_models[0].model_id, mesh ) ) 00707 { 00708 ROS_ERROR( "Failed to get model mesh." ); 00709 return false; 00710 } 00711 00712 ROS_INFO_STREAM( "Model " << model_index << " (database id " 00713 << graspable_object.potential_models[0].model_id << ") has " << mesh.triangles.size()/3 << " triangles and " 00714 << mesh.vertices.size() << " vertices." ); 00715 00716 if ( graspable_object.potential_models[0].pose.header.frame_id != reference_frame_id ) 00717 { 00718 ROS_ERROR( "Model pose has wrong frame id (%s)", 00719 graspable_object.potential_models[0].pose.header.frame_id.c_str() ); 00720 return false; 00721 } 00722 00723 num_recognized++; 00724 } 00725 } 00726 00727 grasp_obj_list.meshes.push_back( mesh ); 00728 grasp_obj_list.graspable_objects.push_back( graspable_object ); 00729 } 00730 00731 //there might be recognized object models without associated cluster (e.g. the stuff that TOD returns) 00732 //so we need to collect those 00733 for ( unsigned model_index=0; model_index<num_models; ++model_index ) 00734 { 00735 //skip all models that we've already stored 00736 if ( used_model_indices.find( model_index ) != used_model_indices.end() ) 00737 { 00738 continue; 00739 } 00740 00741 //only get the mesh if we have recognition results and 00742 //the first one is good enough 00743 if ( recognition_result_.models[ model_index ].model_list.size() == 0 || 00744 recognition_result_.models[ model_index ].model_list[0].confidence > min_marker_quality_ ) 00745 { 00746 ROS_INFO( "Model %d was not recognized.", model_index ); 00747 continue; 00748 } 00749 00750 used_model_indices.insert( model_index ); 00751 00752 object_manipulation_msgs::GraspableObject graspable_object; 00753 arm_navigation_msgs::Shape mesh; 00754 00755 graspable_object.reference_frame_id = reference_frame_id; 00756 graspable_object.potential_models = recognition_result_.models[ model_index ].model_list; 00757 00758 if ( !getModelMesh( graspable_object.potential_models[0].model_id, mesh ) ) 00759 { 00760 ROS_ERROR( "Failed to get mesh for model #%d.", model_index ); 00761 return false; 00762 } 00763 00764 ROS_INFO_STREAM( "Model " << model_index << " (database id " 00765 << graspable_object.potential_models[0].model_id << ") has " << mesh.triangles.size()/3 << " triangles and " 00766 << mesh.vertices.size() << " vertices." ); 00767 00768 if ( graspable_object.potential_models[0].pose.header.frame_id != reference_frame_id ) 00769 { 00770 ROS_ERROR( "Model pose has wrong frame id (%s)", graspable_object.potential_models[0].pose.header.frame_id.c_str() ); 00771 return false; 00772 } 00773 00774 num_recognized++; 00775 00776 //hack to take care the timestamp doesn't get too old 00777 graspable_object.potential_models[0].pose.header.stamp = ros::Time(); 00778 00779 grasp_obj_list.meshes.push_back( mesh ); 00780 grasp_obj_list.graspable_objects.push_back( graspable_object ); 00781 } 00782 00783 result_publisher_.publish( grasp_obj_list ); 00784 00785 printObjects( grasp_obj_list.graspable_objects ); 00786 00787 publishFitMarkers( recognition_result_.models, segmentation_result_.table ); 00788 00789 return true; 00790 } 00791 00792 void InteractiveObjDetBackend::resetMarkers() 00793 { 00794 //delete all published fit markers 00795 for ( size_t i=0; i<delete_markers_.size(); i++ ) 00796 { 00797 delete_markers_[i].header.stamp = ros::Time::now(); 00798 marker_pub_.publish( delete_markers_[i] ); 00799 } 00800 delete_markers_.clear(); 00801 00802 //publish an empty list of objects so interactive_marker_node clears its object_handlers_ list 00803 GraspableObjectList grasp_obj_list; 00804 result_publisher_.publish( grasp_obj_list ); 00805 00806 finishAction( "reset all segmentation and detection markers" ); 00807 } 00808 00809 void InteractiveObjDetBackend::publishFitMarkers( 00810 const std::vector<household_objects_database_msgs::DatabaseModelPoseList> &potential_models, 00811 const tabletop_object_detector::Table &table ) 00812 { 00813 //first, clear the old markers 00814 for ( size_t i=0; i<delete_markers_.size(); i++ ) 00815 { 00816 delete_markers_[i].header.stamp = ros::Time::now(); 00817 marker_pub_.publish( delete_markers_[i] ); 00818 } 00819 delete_markers_.clear(); 00820 00821 for (size_t i=0; i<potential_models.size(); i++) 00822 { 00823 const std::vector<household_objects_database_msgs::DatabaseModelPose> models = potential_models[i].model_list; 00824 for (size_t j=0; j<models.size(); j++) 00825 { 00826 arm_navigation_msgs::Shape mesh; 00827 00828 if ( !getModelMesh( models[j].model_id, mesh ) ) 00829 { 00830 ROS_ERROR("Failed to call database get mesh service for marker display"); 00831 } 00832 else 00833 { 00834 double rank = ((double)j) / std::max( (int)(models.size())-1, 1 ); 00835 visualization_msgs::Marker fitMarker = MarkerGenerator::getFitMarker(mesh, rank); 00836 fitMarker.header = models[j].pose.header; 00837 fitMarker.pose = models[j].pose.pose; 00838 fitMarker.ns = "pr2_interactive_object_detection_model_" + boost::lexical_cast<std::string>(j); 00839 fitMarker.id = current_marker_id_++; 00840 marker_pub_.publish(fitMarker); 00841 00842 //save a marker that will delete this one later.. 00843 visualization_msgs::Marker delete_marker; 00844 delete_marker.header.frame_id = fitMarker.header.frame_id; 00845 delete_marker.id = fitMarker.id; 00846 delete_marker.ns = fitMarker.ns; 00847 delete_marker.action = visualization_msgs::Marker::DELETE; 00848 delete_markers_.push_back( delete_marker ); 00849 } 00850 } 00851 } 00852 } 00853 00854 bool InteractiveObjDetBackend::getPose( geometry_msgs::Pose &pose, std::string from_frame, std::string to_frame ) 00855 { 00856 //we expect the robot to not move during the whole procedure, 00857 //so time::now should do it 00858 ros::Time pose_time = ros::Time::now(); 00859 00860 tf::StampedTransform pose_transform; 00861 try 00862 { 00863 tf::Transformer transformer; 00864 ROS_INFO_STREAM( "Looking up transform from " << from_frame << " to " << to_frame ); 00865 tf_listener_.waitForTransform(to_frame, from_frame, pose_time, ros::Duration(3.0)); 00866 tf_listener_.lookupTransform(to_frame, from_frame, pose_time, pose_transform); 00867 } 00868 catch (tf::TransformException ex) 00869 { 00870 ROS_ERROR("%s",ex.what()); 00871 return false; 00872 } 00873 00874 //convert back 00875 tf::poseTFToMsg ( pose_transform , pose ); 00876 ROS_INFO( "Position: %f, %f, %f / orientation: %f %f %f %f ", 00877 pose.position.x, pose.position.y, pose.position.z, 00878 pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z ); 00879 00880 return true; 00881 } 00882 00883 00884 00885 bool InteractiveObjDetBackend::transformPose( geometry_msgs::PoseStamped &pose, std::string target_frame ) 00886 { 00887 // convert object pose to tf format 00888 btTransform old_pose; 00889 tf::poseMsgToTF ( pose.pose, old_pose ); 00890 00891 //get transformation from object's parent frame to camera frame 00892 std::string old_frame = pose.header.frame_id; 00893 00894 //we expect the robot to not move during the whole procedure, 00895 //so time::now should do it 00896 ros::Time pose_time = ros::Time::now(); 00897 00898 tf::StampedTransform old_to_target; 00899 try 00900 { 00901 tf::Transformer transformer; 00902 ROS_INFO_STREAM( "Looking up transform from " << old_frame << " to " << target_frame ); 00903 tf_listener_.waitForTransform(target_frame, old_frame, pose_time, ros::Duration(3.0)); 00904 tf_listener_.lookupTransform(target_frame, old_frame, pose_time, old_to_target); 00905 } 00906 catch (tf::TransformException ex) 00907 { 00908 ROS_ERROR("%s",ex.what()); 00909 return false; 00910 } 00911 00912 //concatenate transformations 00913 btTransform target_pose = old_to_target * old_pose; 00914 00915 ROS_INFO( "Old position %f, %f, %f / orientation %f %f %f %f ", 00916 pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, 00917 pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z ); 00918 00919 //convert back 00920 tf::poseTFToMsg ( target_pose , pose.pose ); 00921 pose.header.frame_id = target_frame; 00922 00923 ROS_INFO( "New position %f, %f, %f / orientation %f %f %f %f ", 00924 pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, 00925 pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z ); 00926 00927 return true; 00928 } 00929 00930 00931 bool InteractiveObjDetBackend::getModelMesh( int model_id, arm_navigation_msgs::Shape& mesh ) 00932 { 00933 GetModelMesh mesh_srv; 00934 00935 mesh_srv.request.model_id = model_id; 00936 if ( !get_model_mesh_client_.call(mesh_srv) ) 00937 { 00938 ROS_ERROR("Failed to call get model mesh service"); 00939 return false; 00940 } 00941 00942 if (mesh_srv.response.return_code.code != DatabaseReturnCode::SUCCESS) 00943 { 00944 ROS_ERROR("Model mesh service reports an error (code %d)", mesh_srv.response.return_code.code); 00945 return false; 00946 } 00947 00948 mesh = mesh_srv.response.mesh; 00949 return true; 00950 } 00951 00952 00953 bool InteractiveObjDetBackend::getModelInfo(const DatabaseModelPose &model_pose, 00954 std::string &name, std::string &all_tags) 00955 { 00956 GetModelDescription desc; 00957 desc.request.model_id = model_pose.model_id; 00958 if ( !get_model_description_client_.call(desc) ) 00959 { 00960 ROS_ERROR("Failed to call get model description service"); 00961 return false; 00962 } 00963 00964 if (desc.response.return_code.code != desc.response.return_code.SUCCESS ) 00965 { 00966 ROS_ERROR("Model description service reports an error (code %d)", desc.response.return_code.code); 00967 return false; 00968 } 00969 name = desc.response.name; 00970 for (size_t i=0; i<desc.response.tags.size(); i++) 00971 { 00972 if (!all_tags.empty()) all_tags.append(","); 00973 all_tags.append(desc.response.tags.at(i)); 00974 } 00975 return true; 00976 } 00977 00978 00979 int InteractiveObjDetBackend::printObjects(const std::vector<object_manipulation_msgs::GraspableObject> &objects) 00980 { 00981 ROS_INFO_STREAM( "Detected " << objects.size() << " graspable object(s):\n" ); 00982 for (size_t m=0; m<objects.size(); m++) 00983 { 00984 std::string name, all_tags; 00985 if (!objects[m].potential_models.empty()) 00986 { 00987 if (getModelInfo(objects[m].potential_models[0], name, all_tags)) 00988 { 00989 ROS_INFO(" (%d): %s (tags: %s) in frame %s", (int)m, name.c_str(), all_tags.c_str(), 00990 objects[m].potential_models[0].pose.header.frame_id.c_str()); 00991 } 00992 else 00993 { 00994 ROS_INFO(" (%d): database object, details not available.", (int)m ); 00995 } 00996 } 00997 else 00998 { 00999 ROS_INFO(" (%d): unrecognized cluster with %d points", (int)m, 01000 (unsigned int)objects[m].cluster.points.size()); 01001 } 01002 } 01003 return 0; 01004 } 01005 01006 01007 bool InteractiveObjDetBackend::addTableToCollisionMap(tabletop_object_detector::Table table) 01008 { 01009 // make sure collision services are available 01010 ROS_INFO("interactive object detection: waiting for collision map services"); 01011 ros::Time start_time = ros::Time::now(); 01012 while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 01013 { 01014 if (ros::Time::now() - start_time >= ros::Duration(5.0)) 01015 { 01016 ROS_ERROR("collision map services not found"); 01017 return false; 01018 } 01019 } 01020 collision_map_interface_.processCollisionGeometryForTable(table, "table"); 01021 return true; 01022 } 01023 01024 01025 int main(int argc, char **argv) 01026 { 01027 ros::init(argc, argv, "pr2_interactive_object_detection_backend"); 01028 InteractiveObjDetBackend node; 01029 ros::spin(); 01030 return 0; 01031 }