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 #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
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
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
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
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
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
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
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, point_cloud_.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
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
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
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
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
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
00494 if ( !doAutoRecognition() ) return false;
00495
00496 if (user_command_server_->isPreemptRequested()) return false;
00497
00498
00499 if ( interactive )
00500 {
00501 if ( !doInteractiveRecognition() ) return false;
00502 }
00503
00504 if (user_command_server_->isPreemptRequested()) return false;
00505
00506
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
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
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
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
00565 ROS_INFO( "Tabletop detection returned %d clusters.", (int)tabletop_detection_srv.response.detection.clusters.size() );
00566
00567 segmentation_result_.clusters = tabletop_detection_srv.response.detection.clusters;
00568 recognition_result_.cluster_model_indices = tabletop_detection_srv.response.detection.cluster_model_indices;
00569 recognition_result_.models = tabletop_detection_srv.response.detection.models;
00570
00571
00572
00573
00574
00575 return true;
00576 }
00577
00578
00579 bool InteractiveObjDetBackend::doDetection( bool interactive )
00580 {
00581
00582 if ( !doAutoDetection() ) return false;
00583
00584 if (user_command_server_->isPreemptRequested()) return false;
00585
00586 if (!getSensorData(ros::Duration(10.0))) return false;
00587
00588
00589 if ( interactive )
00590 {
00591 if ( !doInteractiveRecognition() ) return false;
00592 }
00593
00594 if (user_command_server_->isPreemptRequested()) return false;
00595
00596
00597 ROS_INFO( "Object detection procedure finished. Publishing result." );
00598 int num_recognized;
00599
00600 if ( !publishResult( num_recognized ) )
00601 {
00602 abortAction( "An error occured while gathering the result data." );
00603 return false;
00604 }
00605
00606 std::ostringstream s;
00607 s << num_recognized << " of " << segmentation_result_.clusters.size() << " objects recognized.";
00608 finishAction( s.str() );
00609
00610 return true;
00611 }
00612
00613 bool InteractiveObjDetBackend::publishResult( int &num_recognized )
00614 {
00615 if ( segmentation_result_.clusters.size() == 0 && recognition_result_.models.size() == 0 )
00616 {
00617 ROS_ERROR( "No results available for publishing!" );
00618 return false;
00619 }
00620
00621 num_recognized = 0;
00622 GraspableObjectList grasp_obj_list;
00623
00624 std::string reference_frame_id;
00625
00626 if ( segmentation_result_.clusters.size() > 0 )
00627 {
00628 reference_frame_id = segmentation_result_.clusters[0].header.frame_id;
00629 }
00630 else
00631 {
00632 if ( recognition_result_.models[0].model_list.size() == 0 )
00633 {
00634 ROS_ERROR( "Model list 0 from recognition result is empty!" );
00635 return false;
00636 }
00637 reference_frame_id = recognition_result_.models[0].model_list[0].pose.header.frame_id;
00638 }
00639
00640 ROS_INFO( "The reference frame is %s", reference_frame_id.c_str() );
00641
00642 grasp_obj_list.camera_info = camera_info_;
00643 grasp_obj_list.image = image_;
00644 getPose( grasp_obj_list.reference_to_camera, reference_frame_id, point_cloud_.header.frame_id );
00645
00646 unsigned num_clusters = segmentation_result_.clusters.size();
00647 unsigned num_models = recognition_result_.models.size();
00648
00649 std::set<unsigned> used_model_indices;
00650 std::set<unsigned> used_cluster_indices;
00651
00652 ROS_INFO( "Number of clusters: %d models: %d", num_clusters, num_models );
00653
00654
00655
00656 for ( unsigned cluster_index=0; cluster_index<num_clusters; ++cluster_index )
00657 {
00658 object_manipulation_msgs::GraspableObject graspable_object;
00659 arm_navigation_msgs::Shape mesh;
00660
00661
00662 graspable_object.cluster = segmentation_result_.clusters[cluster_index];
00663 graspable_object.reference_frame_id = reference_frame_id;
00664
00665
00666 if ( recognition_result_.cluster_model_indices.size() == num_clusters )
00667 {
00668 unsigned model_index = recognition_result_.cluster_model_indices[cluster_index];
00669
00670
00671
00672
00673 if ( used_model_indices.find( model_index ) != used_model_indices.end() )
00674 {
00675 continue;
00676 }
00677
00678 used_model_indices.insert( model_index );
00679
00680 if ( model_index > recognition_result_.models.size() )
00681 {
00682 ROS_ERROR( "Invalid model index for cluster %d!", cluster_index );
00683 return false;
00684 }
00685
00686 graspable_object.cluster = segmentation_result_.clusters[cluster_index];
00687 graspable_object.reference_frame_id = robot_reference_frame_id_;
00688
00689
00690
00691 if ( recognition_result_.models[ model_index ].model_list.size() == 0 ||
00692 recognition_result_.models[ model_index ].model_list[0].confidence > min_marker_quality_ )
00693 {
00694 ROS_INFO( "Model %d was not recognized.", model_index );
00695
00696
00697 if ( graspable_object.cluster.header.frame_id != reference_frame_id )
00698 {
00699 ROS_ERROR( "Point cluster has wrong frame id (%s)", graspable_object.cluster.header.frame_id.c_str() );
00700 return false;
00701 }
00702 }
00703 else
00704 {
00705 graspable_object.potential_models = recognition_result_.models[ model_index ].model_list;
00706
00707 if ( !getModelMesh( graspable_object.potential_models[0].model_id, mesh ) )
00708 {
00709 ROS_ERROR( "Failed to get model mesh." );
00710 return false;
00711 }
00712
00713 ROS_INFO_STREAM( "Model " << model_index << " (database id "
00714 << graspable_object.potential_models[0].model_id << ") has " << mesh.triangles.size()/3 << " triangles and "
00715 << mesh.vertices.size() << " vertices." );
00716
00717 if ( graspable_object.potential_models[0].pose.header.frame_id != reference_frame_id )
00718 {
00719 ROS_ERROR( "Model pose has wrong frame id (%s)",
00720 graspable_object.potential_models[0].pose.header.frame_id.c_str() );
00721 return false;
00722 }
00723
00724 num_recognized++;
00725 }
00726 }
00727
00728 grasp_obj_list.meshes.push_back( mesh );
00729 grasp_obj_list.graspable_objects.push_back( graspable_object );
00730 }
00731
00732
00733
00734 for ( unsigned model_index=0; model_index<num_models; ++model_index )
00735 {
00736
00737 if ( used_model_indices.find( model_index ) != used_model_indices.end() )
00738 {
00739 continue;
00740 }
00741
00742
00743
00744 if ( recognition_result_.models[ model_index ].model_list.size() == 0 ||
00745 recognition_result_.models[ model_index ].model_list[0].confidence > min_marker_quality_ )
00746 {
00747 ROS_INFO( "Model %d was not recognized.", model_index );
00748 continue;
00749 }
00750
00751 used_model_indices.insert( model_index );
00752
00753 object_manipulation_msgs::GraspableObject graspable_object;
00754 arm_navigation_msgs::Shape mesh;
00755
00756 graspable_object.reference_frame_id = reference_frame_id;
00757 graspable_object.potential_models = recognition_result_.models[ model_index ].model_list;
00758
00759 if ( !getModelMesh( graspable_object.potential_models[0].model_id, mesh ) )
00760 {
00761 ROS_ERROR( "Failed to get mesh for model #%d.", model_index );
00762 return false;
00763 }
00764
00765 ROS_INFO_STREAM( "Model " << model_index << " (database id "
00766 << graspable_object.potential_models[0].model_id << ") has " << mesh.triangles.size()/3 << " triangles and "
00767 << mesh.vertices.size() << " vertices." );
00768
00769 if ( graspable_object.potential_models[0].pose.header.frame_id != reference_frame_id )
00770 {
00771 ROS_ERROR( "Model pose has wrong frame id (%s)", graspable_object.potential_models[0].pose.header.frame_id.c_str() );
00772 return false;
00773 }
00774
00775 num_recognized++;
00776
00777
00778 graspable_object.potential_models[0].pose.header.stamp = ros::Time();
00779
00780 grasp_obj_list.meshes.push_back( mesh );
00781 grasp_obj_list.graspable_objects.push_back( graspable_object );
00782 }
00783
00784 result_publisher_.publish( grasp_obj_list );
00785
00786 printObjects( grasp_obj_list.graspable_objects );
00787
00788 publishFitMarkers( recognition_result_.models, segmentation_result_.table );
00789
00790 return true;
00791 }
00792
00793 void InteractiveObjDetBackend::resetMarkers()
00794 {
00795
00796 try
00797 {
00798 for ( size_t i=0; i<delete_markers_.size(); i++ )
00799 {
00800 delete_markers_[i].header.stamp = ros::Time::now();
00801 marker_pub_.publish( delete_markers_[i] );
00802 }
00803 }
00804 catch (...)
00805 {
00806 ROS_ERROR("exception when trying to clear old fit markers!");
00807 }
00808 delete_markers_.clear();
00809
00810
00811 GraspableObjectList grasp_obj_list;
00812 result_publisher_.publish( grasp_obj_list );
00813
00814 finishAction( "reset all segmentation and detection markers" );
00815 }
00816
00817 void InteractiveObjDetBackend::publishFitMarkers(
00818 const std::vector<household_objects_database_msgs::DatabaseModelPoseList> &potential_models,
00819 const tabletop_object_detector::Table &table )
00820 {
00821 try
00822 {
00823
00824 for ( size_t i=0; i<delete_markers_.size(); i++ )
00825 {
00826 delete_markers_[i].header.stamp = ros::Time::now();
00827 marker_pub_.publish( delete_markers_[i] );
00828 }
00829 }
00830 catch (...)
00831 {
00832 ROS_ERROR("exception when trying to clear old fit markers!");
00833 }
00834 delete_markers_.clear();
00835
00836 for (size_t i=0; i<potential_models.size(); i++)
00837 {
00838 const std::vector<household_objects_database_msgs::DatabaseModelPose> models = potential_models[i].model_list;
00839 for (size_t j=0; j<models.size(); j++)
00840 {
00841 arm_navigation_msgs::Shape mesh;
00842
00843 if ( !getModelMesh( models[j].model_id, mesh ) )
00844 {
00845 ROS_ERROR("Failed to call database get mesh service for marker display");
00846 }
00847 else
00848 {
00849 double rank = ((double)j) / std::max( (int)(models.size())-1, 1 );
00850 visualization_msgs::Marker fitMarker = MarkerGenerator::getFitMarker(mesh, rank);
00851 fitMarker.header = models[j].pose.header;
00852 fitMarker.pose = models[j].pose.pose;
00853 fitMarker.ns = "pr2_interactive_object_detection_model_" + boost::lexical_cast<std::string>(j);
00854 fitMarker.id = current_marker_id_++;
00855 marker_pub_.publish(fitMarker);
00856
00857
00858 visualization_msgs::Marker delete_marker;
00859 delete_marker.header.frame_id = fitMarker.header.frame_id;
00860 delete_marker.id = fitMarker.id;
00861 delete_marker.ns = fitMarker.ns;
00862 delete_marker.action = visualization_msgs::Marker::DELETE;
00863 delete_markers_.push_back( delete_marker );
00864 }
00865 }
00866 }
00867 }
00868
00869 bool InteractiveObjDetBackend::getPose( geometry_msgs::Pose &pose, std::string from_frame, std::string to_frame )
00870 {
00871
00872
00873 ros::Time pose_time = ros::Time::now();
00874
00875 tf::StampedTransform pose_transform;
00876 try
00877 {
00878 tf::Transformer transformer;
00879 ROS_INFO_STREAM( "Looking up transform from " << from_frame << " to " << to_frame );
00880 tf_listener_.waitForTransform(to_frame, from_frame, pose_time, ros::Duration(3.0));
00881 tf_listener_.lookupTransform(to_frame, from_frame, pose_time, pose_transform);
00882 }
00883 catch (tf::TransformException ex)
00884 {
00885 ROS_ERROR("%s",ex.what());
00886 return false;
00887 }
00888
00889
00890 tf::poseTFToMsg ( pose_transform , pose );
00891 ROS_INFO( "Position: %f, %f, %f / orientation: %f %f %f %f ",
00892 pose.position.x, pose.position.y, pose.position.z,
00893 pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z );
00894
00895 return true;
00896 }
00897
00898
00899
00900 bool InteractiveObjDetBackend::transformPose( geometry_msgs::PoseStamped &pose, std::string target_frame )
00901 {
00902
00903 tf::Transform old_pose;
00904 tf::poseMsgToTF ( pose.pose, old_pose );
00905
00906
00907 std::string old_frame = pose.header.frame_id;
00908
00909
00910
00911 ros::Time pose_time = ros::Time::now();
00912
00913 tf::StampedTransform old_to_target;
00914 try
00915 {
00916 tf::Transformer transformer;
00917 ROS_INFO_STREAM( "Looking up transform from " << old_frame << " to " << target_frame );
00918 tf_listener_.waitForTransform(target_frame, old_frame, pose_time, ros::Duration(3.0));
00919 tf_listener_.lookupTransform(target_frame, old_frame, pose_time, old_to_target);
00920 }
00921 catch (tf::TransformException ex)
00922 {
00923 ROS_ERROR("%s",ex.what());
00924 return false;
00925 }
00926
00927
00928 tf::Transform target_pose = old_to_target * old_pose;
00929
00930 ROS_INFO( "Old position %f, %f, %f / orientation %f %f %f %f ",
00931 pose.pose.position.x, pose.pose.position.y, pose.pose.position.z,
00932 pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z );
00933
00934
00935 tf::poseTFToMsg ( target_pose , pose.pose );
00936 pose.header.frame_id = target_frame;
00937
00938 ROS_INFO( "New position %f, %f, %f / orientation %f %f %f %f ",
00939 pose.pose.position.x, pose.pose.position.y, pose.pose.position.z,
00940 pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z );
00941
00942 return true;
00943 }
00944
00945
00946 bool InteractiveObjDetBackend::getModelMesh( int model_id, arm_navigation_msgs::Shape& mesh )
00947 {
00948 GetModelMesh mesh_srv;
00949
00950 mesh_srv.request.model_id = model_id;
00951 if ( !get_model_mesh_client_.call(mesh_srv) )
00952 {
00953 ROS_ERROR("Failed to call get model mesh service");
00954 return false;
00955 }
00956
00957 if (mesh_srv.response.return_code.code != DatabaseReturnCode::SUCCESS)
00958 {
00959 ROS_ERROR("Model mesh service reports an error (code %d)", mesh_srv.response.return_code.code);
00960 return false;
00961 }
00962
00963 mesh = mesh_srv.response.mesh;
00964 return true;
00965 }
00966
00967
00968 bool InteractiveObjDetBackend::getModelInfo(const DatabaseModelPose &model_pose,
00969 std::string &name, std::string &all_tags)
00970 {
00971 GetModelDescription desc;
00972 desc.request.model_id = model_pose.model_id;
00973 if ( !get_model_description_client_.call(desc) )
00974 {
00975 ROS_ERROR("Failed to call get model description service");
00976 return false;
00977 }
00978
00979 if (desc.response.return_code.code != desc.response.return_code.SUCCESS )
00980 {
00981 ROS_ERROR("Model description service reports an error (code %d)", desc.response.return_code.code);
00982 return false;
00983 }
00984 name = desc.response.name;
00985 for (size_t i=0; i<desc.response.tags.size(); i++)
00986 {
00987 if (!all_tags.empty()) all_tags.append(",");
00988 all_tags.append(desc.response.tags.at(i));
00989 }
00990 return true;
00991 }
00992
00993
00994 int InteractiveObjDetBackend::printObjects(const std::vector<object_manipulation_msgs::GraspableObject> &objects)
00995 {
00996 ROS_INFO_STREAM( "Detected " << objects.size() << " graspable object(s):\n" );
00997 for (size_t m=0; m<objects.size(); m++)
00998 {
00999 std::string name, all_tags;
01000 if (!objects[m].potential_models.empty())
01001 {
01002 if (getModelInfo(objects[m].potential_models[0], name, all_tags))
01003 {
01004 ROS_INFO(" (%d): %s (tags: %s) in frame %s", (int)m, name.c_str(), all_tags.c_str(),
01005 objects[m].potential_models[0].pose.header.frame_id.c_str());
01006 }
01007 else
01008 {
01009 ROS_INFO(" (%d): database object, details not available.", (int)m );
01010 }
01011 }
01012 else
01013 {
01014 ROS_INFO(" (%d): unrecognized cluster with %d points", (int)m,
01015 (unsigned int)objects[m].cluster.points.size());
01016 }
01017 }
01018 return 0;
01019 }
01020
01021
01022 bool InteractiveObjDetBackend::addTableToCollisionMap(tabletop_object_detector::Table table)
01023 {
01024
01025 ROS_INFO("interactive object detection: waiting for collision map services");
01026 ros::Time start_time = ros::Time::now();
01027 while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0)))
01028 {
01029 if (ros::Time::now() - start_time >= ros::Duration(5.0))
01030 {
01031 ROS_ERROR("collision map services not found");
01032 return false;
01033 }
01034 }
01035 collision_map_interface_.processCollisionGeometryForTable(table, "table");
01036 return true;
01037 }
01038
01039
01040 int main(int argc, char **argv)
01041 {
01042 ros::init(argc, argv, "pr2_interactive_object_detection_backend");
01043 InteractiveObjDetBackend node;
01044 ros::spin();
01045 return 0;
01046 }