$search
00001 #include <ros/ros.h> 00002 00003 #include <actionlib/client/simple_action_client.h> 00004 #include <tabletop_object_detector/TabletopDetection.h> 00005 #include <tabletop_collision_map_processing/TabletopCollisionMapProcessing.h> 00006 #include <object_manipulation_msgs/PickupAction.h> 00007 #include <object_manipulation_msgs/PlaceAction.h> 00008 00009 int main(int argc, char **argv) 00010 { 00011 //initialize the ROS node 00012 ros::init(argc, argv, "pick_and_place_app"); 00013 ros::NodeHandle nh; 00014 00015 //set service and action names 00016 const std::string OBJECT_DETECTION_SERVICE_NAME = "/object_detection"; 00017 const std::string COLLISION_PROCESSING_SERVICE_NAME = 00018 "/tabletop_collision_map_processing/tabletop_collision_map_processing"; 00019 const std::string PICKUP_ACTION_NAME = 00020 "/object_manipulator/object_manipulator_pickup"; 00021 const std::string PLACE_ACTION_NAME = 00022 "/object_manipulator/object_manipulator_place"; 00023 00024 //create service and action clients 00025 ros::ServiceClient object_detection_srv; 00026 ros::ServiceClient collision_processing_srv; 00027 actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> 00028 pickup_client(PICKUP_ACTION_NAME, true); 00029 actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> 00030 place_client(PLACE_ACTION_NAME, true); 00031 00032 //wait for detection client 00033 while ( !ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME, 00034 ros::Duration(2.0)) && nh.ok() ) 00035 { 00036 ROS_INFO("Waiting for object detection service to come up"); 00037 } 00038 if (!nh.ok()) exit(0); 00039 object_detection_srv = 00040 nh.serviceClient<tabletop_object_detector::TabletopDetection> 00041 (OBJECT_DETECTION_SERVICE_NAME, true); 00042 00043 //wait for collision map processing client 00044 while ( !ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME, 00045 ros::Duration(2.0)) && nh.ok() ) 00046 { 00047 ROS_INFO("Waiting for collision processing service to come up"); 00048 } 00049 if (!nh.ok()) exit(0); 00050 collision_processing_srv = 00051 nh.serviceClient 00052 <tabletop_collision_map_processing::TabletopCollisionMapProcessing> 00053 (COLLISION_PROCESSING_SERVICE_NAME, true); 00054 00055 //wait for pickup client 00056 while(!pickup_client.waitForServer(ros::Duration(2.0)) && nh.ok()) 00057 { 00058 ROS_INFO_STREAM("Waiting for action client " << PICKUP_ACTION_NAME); 00059 } 00060 if (!nh.ok()) exit(0); 00061 00062 //wait for place client 00063 while(!place_client.waitForServer(ros::Duration(2.0)) && nh.ok()) 00064 { 00065 ROS_INFO_STREAM("Waiting for action client " << PLACE_ACTION_NAME); 00066 } 00067 if (!nh.ok()) exit(0); 00068 00069 00070 00071 //call the tabletop detection 00072 ROS_INFO("Calling tabletop detector"); 00073 tabletop_object_detector::TabletopDetection detection_call; 00074 //we want recognized database objects returned 00075 //set this to false if you are using the pipeline without the database 00076 detection_call.request.return_clusters = true; 00077 //we want the individual object point clouds returned as well 00078 detection_call.request.return_models = true; 00079 if (!object_detection_srv.call(detection_call)) 00080 { 00081 ROS_ERROR("Tabletop detection service failed"); 00082 return -1; 00083 } 00084 if (detection_call.response.detection.result != 00085 detection_call.response.detection.SUCCESS) 00086 { 00087 ROS_ERROR("Tabletop detection returned error code %d", 00088 detection_call.response.detection.result); 00089 return -1; 00090 } 00091 if (detection_call.response.detection.clusters.empty() && 00092 detection_call.response.detection.models.empty() ) 00093 { 00094 ROS_ERROR("The tabletop detector detected the table, but found no objects"); 00095 return -1; 00096 } 00097 00098 00099 00100 //call collision map processing 00101 ROS_INFO("Calling collision map processing"); 00102 tabletop_collision_map_processing::TabletopCollisionMapProcessing 00103 processing_call; 00104 //pass the result of the tabletop detection 00105 processing_call.request.detection_result = detection_call.response.detection; 00106 //ask for the exising map and collision models to be reset 00107 //processing_call.request.reset_static_map = true; 00108 processing_call.request.reset_collision_models = true; 00109 processing_call.request.reset_attached_models = true; 00110 //ask for a new static collision map to be taken with the laser 00111 //after the new models are added to the environment 00112 //processing_call.request.take_static_collision_map = true; 00113 //ask for the results to be returned in base link frame 00114 processing_call.request.desired_frame = "base_link"; 00115 if (!collision_processing_srv.call(processing_call)) 00116 { 00117 ROS_ERROR("Collision map processing service failed"); 00118 return -1; 00119 } 00120 //the collision map processor returns instances of graspable objects 00121 if (processing_call.response.graspable_objects.empty()) 00122 { 00123 ROS_ERROR("Collision map processing returned no graspable objects"); 00124 return -1; 00125 } 00126 00127 00128 //call object pickup 00129 ROS_INFO("Calling the pickup action"); 00130 object_manipulation_msgs::PickupGoal pickup_goal; 00131 //pass one of the graspable objects returned by the collission map processor 00132 pickup_goal.target = processing_call.response.graspable_objects.at(0); 00133 //pass the name that the object has in the collision environment 00134 //this name was also returned by the collision map processor 00135 pickup_goal.collision_object_name = 00136 processing_call.response.collision_object_names.at(0); 00137 //pass the collision name of the table, also returned by the collision 00138 //map processor 00139 pickup_goal.collision_support_surface_name = 00140 processing_call.response.collision_support_surface_name; 00141 //pick up the object with the right arm 00142 pickup_goal.arm_name = "right_arm"; 00143 //specify the desired distance between pre-grasp and final grasp 00144 //pickup_goal.desired_approach_distance = 0.1; 00145 //pickup_goal.min_approach_distance = 0.05; 00146 //we will be lifting the object along the "vertical" direction 00147 //which is along the z axis in the base_link frame 00148 geometry_msgs::Vector3Stamped direction; 00149 direction.header.stamp = ros::Time::now(); 00150 direction.header.frame_id = "base_link"; 00151 direction.vector.x = 0; 00152 direction.vector.y = 0; 00153 direction.vector.z = 1; 00154 pickup_goal.lift.direction = direction; 00155 //request a vertical lift of 10cm after grasping the object 00156 pickup_goal.lift.desired_distance = 0.1; 00157 pickup_goal.lift.min_distance = 0.05; 00158 //do not use tactile-based grasping or tactile-based lift 00159 pickup_goal.use_reactive_lift = false; 00160 pickup_goal.use_reactive_execution = false; 00161 //send the goal 00162 pickup_client.sendGoal(pickup_goal); 00163 while (!pickup_client.waitForResult(ros::Duration(10.0))) 00164 { 00165 ROS_INFO("Waiting for the pickup action..."); 00166 } 00167 object_manipulation_msgs::PickupResult pickup_result = 00168 *(pickup_client.getResult()); 00169 if (pickup_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) 00170 { 00171 ROS_ERROR("The pickup action has failed with result code %d", 00172 pickup_result.manipulation_result.value); 00173 return -1; 00174 } 00175 00176 00177 00178 //remember where we picked the object up from 00179 geometry_msgs::PoseStamped pickup_location; 00180 if (processing_call.response.graspable_objects.at(0).potential_models.size() > 0) 00181 { 00182 //for database recognized objects, the location of the object 00183 //is encapsulated in GraspableObject the message 00184 pickup_location = 00185 processing_call.response.graspable_objects.at(0).potential_models.at(0).pose; 00186 } 00187 else 00188 { 00189 //for unrecognized point clouds, the location of the object is considered 00190 //to be the origin of the frame that the cluster is in 00191 pickup_location.header = 00192 processing_call.response.graspable_objects.at(0).cluster.header; 00193 //identity pose 00194 pickup_location.pose.orientation.w = 1; 00195 } 00196 //create a place location, offset by 10 cm from the pickup location 00197 geometry_msgs::PoseStamped place_location = pickup_location; 00198 place_location.header.stamp = ros::Time::now(); 00199 place_location.pose.position.x += 0.1; 00200 00201 00202 00203 //put the object down 00204 ROS_INFO("Calling the place action"); 00205 object_manipulation_msgs::PlaceGoal place_goal; 00206 //place at the prepared location 00207 place_goal.place_locations.push_back(place_location); 00208 //the collision names of both the objects and the table 00209 //same as in the pickup action 00210 place_goal.collision_object_name = 00211 processing_call.response.collision_object_names.at(0); 00212 place_goal.collision_support_surface_name = 00213 processing_call.response.collision_support_surface_name; 00214 //information about which grasp was executed on the object, returned by 00215 //the pickup action 00216 place_goal.grasp = pickup_result.grasp; 00217 //use the right rm to place 00218 place_goal.arm_name = "right_arm"; 00219 //padding used when determining if the requested place location 00220 //would bring the object in collision with the environment 00221 place_goal.place_padding = 0.02; 00222 //how much the gripper should retreat after placing the object 00223 place_goal.desired_retreat_distance = 0.1; 00224 place_goal.min_retreat_distance = 0.05; 00225 //we will be putting down the object along the "vertical" direction 00226 //which is along the z axis in the base_link frame 00227 direction.header.stamp = ros::Time::now(); 00228 direction.header.frame_id = "base_link"; 00229 direction.vector.x = 0; 00230 direction.vector.y = 0; 00231 direction.vector.z = -1; 00232 place_goal.approach.direction = direction; 00233 //request a vertical put down motion of 10cm before placing the object 00234 place_goal.approach.desired_distance = 0.1; 00235 place_goal.approach.min_distance = 0.05; 00236 //we are not using tactile based placing 00237 place_goal.use_reactive_place = false; 00238 //send the goal 00239 place_client.sendGoal(place_goal); 00240 while (!place_client.waitForResult(ros::Duration(10.0))) 00241 { 00242 ROS_INFO("Waiting for the place action..."); 00243 } 00244 object_manipulation_msgs::PlaceResult place_result = 00245 *(place_client.getResult()); 00246 if (place_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) 00247 { 00248 ROS_ERROR("Place failed with error code %d", 00249 place_result.manipulation_result.value); 00250 return -1; 00251 } 00252 00253 //success! 00254 ROS_INFO("Success! Object moved."); 00255 return 0; 00256 }