$search
00001 #include <ros/ros.h> 00002 #include <tabletop_object_detector/TabletopDetection.h> 00003 #include <tabletop_collision_map_processing/TabletopCollisionMapProcessing.h> 00004 #include <object_manipulation_msgs/GraspPlanning.h> 00005 #include <visualization_msgs/Marker.h> 00006 00007 int main(int argc, char **argv) 00008 { 00009 //initialize the ROS node 00010 ros::init(argc, argv, "katana_grasp_analysis_test"); 00011 ros::NodeHandle nh; 00012 00013 //set service and action names 00014 const std::string OBJECT_DETECTION_SERVICE_NAME = "/object_detection"; 00015 const std::string COLLISION_PROCESSING_SERVICE_NAME = 00016 "/tabletop_collision_map_processing/tabletop_collision_map_processing"; 00017 const std::string GRASP_PLANNER_SERVICE_NAME = "/openrave_grasp_planner"; 00018 00019 //create service and action clients 00020 ros::ServiceClient object_detection_srv; 00021 ros::ServiceClient collision_processing_srv; 00022 ros::ServiceClient grasp_planner_srv; 00023 00024 //wait for detection client 00025 while (!ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME, ros::Duration(2.0)) && nh.ok()) 00026 { 00027 ROS_INFO("Waiting for object detection service to come up"); 00028 } 00029 if (!nh.ok()) 00030 exit(0); 00031 object_detection_srv = nh.serviceClient<tabletop_object_detector::TabletopDetection> (OBJECT_DETECTION_SERVICE_NAME, 00032 true); 00033 00034 //wait for collision map processing client 00035 while (!ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME, ros::Duration(2.0)) && nh.ok()) 00036 { 00037 ROS_INFO("Waiting for collision processing service to come up"); 00038 } 00039 if (!nh.ok()) 00040 exit(0); 00041 collision_processing_srv 00042 = nh.serviceClient<tabletop_collision_map_processing::TabletopCollisionMapProcessing> ( 00043 COLLISION_PROCESSING_SERVICE_NAME, 00044 true); 00045 00046 //wait for grasp planner client 00047 while (!ros::service::waitForService(GRASP_PLANNER_SERVICE_NAME, ros::Duration(2.0)) && nh.ok()) 00048 { 00049 ROS_INFO("Waiting for grasp planner service to come up"); 00050 } 00051 if (!nh.ok()) 00052 exit(0); 00053 grasp_planner_srv = nh.serviceClient<object_manipulation_msgs::GraspPlanning> (GRASP_PLANNER_SERVICE_NAME, 00054 true); 00055 00056 //call the tabletop detection 00057 ROS_INFO("Calling Tabletop Detector"); 00058 tabletop_object_detector::TabletopDetection detection_call; 00059 //we want recognized database objects returned 00060 //set this to false if you are using the pipeline without the database 00061 detection_call.request.return_clusters = false; 00062 //we want the individual object point clouds returned as well 00063 detection_call.request.return_models = false; 00064 if (!object_detection_srv.call(detection_call)) 00065 { 00066 ROS_ERROR("Tabletop detection service failed"); 00067 return -1; 00068 } 00069 if (detection_call.response.detection.result != detection_call.response.detection.SUCCESS) 00070 { 00071 ROS_ERROR("Tabletop detection returned error code %d", 00072 detection_call.response.detection.result); 00073 return -1; 00074 } 00075 if (detection_call.response.detection.clusters.empty() && detection_call.response.detection.models.empty()) 00076 { 00077 ROS_ERROR("The tabletop detector detected the table, " 00078 "but found no objects"); 00079 return -1; 00080 } 00081 else 00082 { 00083 ROS_WARN("Tabletop suceeded and found %zu unknown clusters and %zu known objects, and the table", detection_call.response.detection.clusters.size(), detection_call.response.detection.models.size()); 00084 } 00085 00086 //call collision map processing 00087 ROS_INFO("Calling collision map processing"); 00088 tabletop_collision_map_processing::TabletopCollisionMapProcessing processing_call; 00089 //pass the result of the tabletop detection 00090 processing_call.request.detection_result = detection_call.response.detection; 00091 //ask for the exising map and collision models to be reset 00092 //processing_call.request.reset_static_map = true; 00093 processing_call.request.reset_collision_models = true; 00094 processing_call.request.reset_attached_models = true; 00095 //ask for a new static collision map to be taken with the laser 00096 //after the new models are added to the environment 00097 //processing_call.request.take_static_collision_map = false; 00098 //ask for the results to be returned in base link frame 00099 processing_call.request.desired_frame = "katana_base_link"; 00100 if (!collision_processing_srv.call(processing_call)) 00101 { 00102 ROS_ERROR("Collision map processing service failed"); 00103 return -1; 00104 } 00105 //the collision map processor returns instances of graspable objects 00106 if (processing_call.response.graspable_objects.empty()) 00107 { 00108 ROS_ERROR("Collision map processing returned no graspable objects"); 00109 return -1; 00110 } 00111 else 00112 { 00113 ROS_WARN("Collision map processing succeeded and returned %zu graspable objects", processing_call.response.graspable_objects.size() ); 00114 } 00115 00116 ROS_ERROR("und weiter"); 00117 00118 //call the planner and save the list of grasps 00119 object_manipulation_msgs::GraspPlanning srv; 00120 srv.request.arm_name = "arm"; 00121 00122 ROS_INFO("taking first object with:"); 00123 ROS_INFO("reference_frame: %s", processing_call.response.graspable_objects.at(0).reference_frame_id.c_str()); 00124 ROS_INFO("cluster_frame: %s", processing_call.response.graspable_objects.at(0).cluster.header.frame_id.c_str()); 00125 00126 srv.request.target = processing_call.response.graspable_objects.at(0); 00127 //srv.request.collision_object_name = pickup_goal->collision_object_name; 00128 //srv.request.collision_support_surface_name = processing_call.response.collision_support_surface_name; 00129 00130 if (!grasp_planner_srv.call(srv)) 00131 { 00132 ROS_ERROR("Object manipulator failed to call grasp planner at"); // %s"), grasp_planner_srv.c_str()); 00133 // result.manipulation_result.value = ManipulationResult::ERROR; 00134 // action_server->setAborted(result); 00135 return 0; 00136 } 00137 00138 if (srv.response.error_code.value != srv.response.error_code.SUCCESS) 00139 { 00140 ROS_ERROR("Object manipulator: grasp planner failed with error code %d", srv.response.error_code.value); 00141 // result.manipulation_result.value = ManipulationResult::ERROR; 00142 //action_server->setAborted(result); 00143 return 0; 00144 } 00145 00146 00147 //object_manipulation_msgs::Grasp grasps[] = srv.response.grasps; 00148 if (srv.response.error_code.value == srv.response.error_code.SUCCESS) 00149 { 00150 ROS_INFO("Calling the GraspPlanner succeded"); 00151 } 00152 00153 object_manipulation_msgs::Grasp current_grasp = srv.response.grasps.at(0); 00154 00155 ROS_INFO("Current Grasp at: %f % f % f // %f %f %f %f", 00156 current_grasp.grasp_pose.position.x, 00157 current_grasp.grasp_pose.position.y, 00158 current_grasp.grasp_pose.position.z, 00159 current_grasp.grasp_pose.orientation.x, 00160 current_grasp.grasp_pose.orientation.y, 00161 current_grasp.grasp_pose.orientation.z, 00162 current_grasp.grasp_pose.orientation.w); 00163 00164 }