$search
00001 #include <ros/ros.h> 00002 00003 #include <tf/transform_listener.h> 00004 #include <actionlib/client/simple_action_client.h> 00005 #include <tabletop_object_detector/TabletopDetection.h> 00006 #include <tabletop_collision_map_processing/TabletopCollisionMapProcessing.h> 00007 #include <object_manipulation_msgs/PickupAction.h> 00008 #include <object_manipulation_msgs/PlaceAction.h> 00009 #include <arm_navigation_msgs/MoveArmAction.h> 00010 #include <std_srvs/Empty.h> 00011 00012 static const double TABLE_HEIGHT = 0.28; 00013 00014 tf::TransformListener *tf_listener; 00015 00016 00017 static const size_t NUM_JOINTS = 5; 00018 00019 bool move_to_joint_goal(std::vector<arm_navigation_msgs::JointConstraint> joint_constraints, 00020 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> &move_arm) { 00021 00022 arm_navigation_msgs::MoveArmGoal goal; 00023 00024 goal.motion_plan_request.group_name = "arm"; 00025 goal.motion_plan_request.num_planning_attempts = 1; 00026 goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0); 00027 00028 goal.motion_plan_request.planner_id = std::string(""); 00029 goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path"); 00030 00031 goal.motion_plan_request.goal_constraints.joint_constraints = joint_constraints; 00032 00033 00034 bool finished_within_time = false; 00035 move_arm.sendGoal(goal); 00036 finished_within_time = move_arm.waitForResult(ros::Duration(40.0)); 00037 if (!finished_within_time) 00038 { 00039 move_arm.cancelGoal(); 00040 ROS_INFO("Timed out achieving goal!"); 00041 return false; 00042 } 00043 else 00044 { 00045 actionlib::SimpleClientGoalState state = move_arm.getState(); 00046 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); 00047 if (success) 00048 ROS_INFO("Action finished: %s",state.toString().c_str()); 00049 else 00050 ROS_INFO("Action failed: %s",state.toString().c_str()); 00051 00052 return success; 00053 } 00054 } 00055 00056 00060 bool nearest_object(std::vector<object_manipulation_msgs::GraspableObject>& objects, geometry_msgs::PointStamped& reference_point, int& object_ind) 00061 { 00062 geometry_msgs::PointStamped point; 00063 00064 // convert point to base_link frame 00065 tf_listener->transformPoint("/base_link", reference_point, point); 00066 00067 // find the closest object 00068 double nearest_dist = 1e6; 00069 int nearest_object_ind = -1; 00070 00071 for (size_t i = 0; i < objects.size(); ++i) 00072 { 00073 sensor_msgs::PointCloud cloud; 00074 tf_listener->transformPointCloud("/base_link", objects[i].cluster, cloud); 00075 00076 // calculate average 00077 float x = 0.0, y = 0.0, z = 0.0; 00078 for (size_t j = 0; j < cloud.points.size(); ++j) 00079 { 00080 x += cloud.points[j].x; 00081 y += cloud.points[j].y; 00082 z += cloud.points[j].z; 00083 } 00084 x /= cloud.points.size(); 00085 y /= cloud.points.size(); 00086 z /= cloud.points.size(); 00087 00088 double dist = sqrt(pow(x - point.point.x, 2.0) + pow(y - point.point.y, 2.0) + pow(z - point.point.z, 2.0)); 00089 if (dist < nearest_dist) 00090 { 00091 nearest_dist = dist; 00092 nearest_object_ind = i; 00093 } 00094 } 00095 00096 if (nearest_object_ind > -1) 00097 { 00098 ROS_INFO("nearest object ind: %d (distance: %f)", nearest_object_ind, nearest_dist); 00099 object_ind = nearest_object_ind; 00100 return true; 00101 } else 00102 { 00103 ROS_ERROR("No nearby objects. Unable to select grasp target"); 00104 return false; 00105 } 00106 } 00107 00108 int main(int argc, char **argv) 00109 { 00110 //initialize the ROS node 00111 ros::init(argc, argv, "pick_and_place_app"); 00112 ros::NodeHandle nh; 00113 00114 //set service and action names 00115 const std::string OBJECT_DETECTION_SERVICE_NAME = "/object_detection"; 00116 const std::string COLLISION_PROCESSING_SERVICE_NAME = "/tabletop_collision_map_processing/tabletop_collision_map_processing"; 00117 const std::string PICKUP_ACTION_NAME = "/object_manipulator/object_manipulator_pickup"; 00118 const std::string PLACE_ACTION_NAME = "/object_manipulator/object_manipulator_place"; 00119 const std::string MOVE_ARM_ACTION_NAME = "/move_arm"; 00120 const std::string COLLIDER_RESET_SERVICE_NAME = "/collider_node/reset"; 00121 00122 // create TF listener 00123 tf_listener = new tf::TransformListener(); 00124 00125 //create service and action clients 00126 ros::ServiceClient object_detection_srv; 00127 ros::ServiceClient collision_processing_srv; 00128 ros::ServiceClient collider_reset_srv; 00129 actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> 00130 pickup_client(PICKUP_ACTION_NAME, true); 00131 actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> 00132 place_client(PLACE_ACTION_NAME, true); 00133 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(MOVE_ARM_ACTION_NAME, true); 00134 00135 //wait for detection client 00136 while ( !ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME, 00137 ros::Duration(2.0)) && nh.ok() ) 00138 { 00139 ROS_INFO("Waiting for object detection service to come up"); 00140 } 00141 if (!nh.ok()) exit(0); 00142 object_detection_srv = 00143 nh.serviceClient<tabletop_object_detector::TabletopDetection> 00144 (OBJECT_DETECTION_SERVICE_NAME, true); 00145 00146 //wait for collision map processing client 00147 while (!ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME, ros::Duration(2.0)) && nh.ok()) 00148 { 00149 ROS_INFO("Waiting for collision processing service to come up"); 00150 } 00151 if (!nh.ok()) 00152 exit(0); 00153 collision_processing_srv = nh.serviceClient<tabletop_collision_map_processing::TabletopCollisionMapProcessing>( 00154 COLLISION_PROCESSING_SERVICE_NAME, true); 00155 00156 //wait for collision map processing client 00157 while (!ros::service::waitForService(COLLIDER_RESET_SERVICE_NAME, ros::Duration(2.0)) && nh.ok()) 00158 { 00159 ROS_INFO("Waiting for collider reset service to come up"); 00160 } 00161 if (!nh.ok()) 00162 exit(0); 00163 collider_reset_srv = nh.serviceClient<std_srvs::Empty>( 00164 COLLIDER_RESET_SERVICE_NAME, true); 00165 00166 //wait for pickup client 00167 while(!pickup_client.waitForServer(ros::Duration(2.0)) && nh.ok()) 00168 { 00169 ROS_INFO_STREAM("Waiting for action client " << PICKUP_ACTION_NAME); 00170 } 00171 if (!nh.ok()) exit(0); 00172 00173 //wait for place client 00174 while(!place_client.waitForServer(ros::Duration(2.0)) && nh.ok()) 00175 { 00176 ROS_INFO_STREAM("Waiting for action client " << PLACE_ACTION_NAME); 00177 } 00178 if (!nh.ok()) exit(0); 00179 00180 //wait for move_arm action client 00181 while(!move_arm.waitForServer(ros::Duration(2.0)) && nh.ok()) 00182 { 00183 ROS_INFO_STREAM("Waiting for action client " << MOVE_ARM_ACTION_NAME); 00184 } 00185 if (!nh.ok()) exit(0); 00186 00187 00188 00189 00190 00191 // ----- reset collision map 00192 ROS_INFO("Clearing collision map"); 00193 std_srvs::Empty empty; 00194 if (!collider_reset_srv.call(empty)) 00195 { 00196 ROS_ERROR("Collider reset service failed"); 00197 return -1; 00198 } 00199 ros::Duration(5.0).sleep(); // wait for collision map to be completely cleared 00200 00201 // ----- move arm out of the way 00202 std::vector<std::string> names(NUM_JOINTS); 00203 names[0] = "katana_motor1_pan_joint"; 00204 names[1] = "katana_motor2_lift_joint"; 00205 names[2] = "katana_motor3_lift_joint"; 00206 names[3] = "katana_motor4_lift_joint"; 00207 names[4] = "katana_motor5_wrist_roll_joint"; 00208 00209 00210 std::vector<arm_navigation_msgs::JointConstraint> joint_constraints(NUM_JOINTS); 00211 00212 for (size_t i = 0; i < NUM_JOINTS; ++i) 00213 { 00214 joint_constraints[i].joint_name = names[i]; 00215 joint_constraints[i].tolerance_below = 0.1; 00216 joint_constraints[i].tolerance_above = 0.1; 00217 } 00218 00219 // - post_calibration_posture: 00220 // name: ['katana_motor1_pan_joint', 'katana_motor2_lift_joint', 'katana_motor3_lift_joint', 'katana_motor4_lift_joint', 'katana_motor5_wrist_roll_joint'] 00221 // position: [-2.9641690268167444, 2.13549384276445, -2.1556486321117725, -1.971949347057968, -2.9318804356548496] 00222 // - arm_away_posture: 00223 // name: ['katana_motor1_pan_joint', 'katana_motor2_lift_joint', 'katana_motor3_lift_joint', 'katana_motor4_lift_joint', 'katana_motor5_wrist_roll_joint', 'katana_r_finger_joint', 'katana_l_finger_joint'] 00224 // position: [0.0, 2.13549384276445, -2.1556486321117725, -1.971949347057968, 0.0] 00225 00226 joint_constraints[0].position = 0.0; 00227 joint_constraints[1].position = 2.13549384276445; 00228 joint_constraints[2].position = -2.1556486321117725; 00229 joint_constraints[3].position = -1.971949347057968; 00230 joint_constraints[4].position = 0.0; 00231 00232 ROS_INFO("Moving arm away"); 00233 bool success; 00234 success = move_to_joint_goal(joint_constraints, move_arm); 00235 if (!success) 00236 return -1; 00237 00238 00239 // ----- call the tabletop detection 00240 ROS_INFO("Calling tabletop detector"); 00241 tabletop_object_detector::TabletopDetection detection_call; 00242 //we want recognized database objects returned 00243 //set this to false if you are using the pipeline without the database 00244 detection_call.request.return_clusters = true; 00245 //we want the individual object point clouds returned as well 00246 detection_call.request.return_models = false; 00247 if (!object_detection_srv.call(detection_call)) 00248 { 00249 ROS_ERROR("Tabletop detection service failed"); 00250 return -1; 00251 } 00252 if (detection_call.response.detection.result != 00253 detection_call.response.detection.SUCCESS) 00254 { 00255 ROS_ERROR("Tabletop detection returned error code %d", 00256 detection_call.response.detection.result); 00257 return -1; 00258 } 00259 if (detection_call.response.detection.clusters.empty() && 00260 detection_call.response.detection.models.empty() ) 00261 { 00262 ROS_ERROR("The tabletop detector detected the table, but found no objects"); 00263 return -1; 00264 } 00265 00266 00267 // ----- call collision map processing 00268 ROS_INFO("Calling collision map processing"); 00269 tabletop_collision_map_processing::TabletopCollisionMapProcessing 00270 processing_call; 00271 //pass the result of the tabletop detection 00272 processing_call.request.detection_result = detection_call.response.detection; 00273 //ask for the exising map and collision models to be reset 00274 //processing_call.request.reset_static_map = true; 00275 processing_call.request.reset_collision_models = true; 00276 processing_call.request.reset_attached_models = true; 00277 //ask for a new static collision map to be taken with the laser 00278 //after the new models are added to the environment 00279 //processing_call.request.take_static_collision_map = true; 00280 //ask for the results to be returned in base link frame 00281 processing_call.request.desired_frame = "katana_base_link"; 00282 if (!collision_processing_srv.call(processing_call)) 00283 { 00284 ROS_ERROR("Collision map processing service failed"); 00285 return -1; 00286 } 00287 //the collision map processor returns instances of graspable objects 00288 if (processing_call.response.graspable_objects.empty()) 00289 { 00290 ROS_ERROR("Collision map processing returned no graspable objects"); 00291 return -1; 00292 } 00293 00294 00295 // ----- pick up object near point: 25 cm in front and 15 cm to the right of the robot 00296 geometry_msgs::PointStamped pickup_point; 00297 pickup_point.header.frame_id = "/base_footprint"; 00298 pickup_point.point.x = 0.25; 00299 pickup_point.point.y = -0.15; 00300 pickup_point.point.z = TABLE_HEIGHT; 00301 00302 //call object pickup 00303 ROS_INFO("Calling the pickup action"); 00304 object_manipulation_msgs::PickupGoal pickup_goal; 00305 //pass one of the graspable objects returned by the collission map processor 00306 int object_to_pick_ind; 00307 00308 if (!nearest_object(processing_call.response.graspable_objects, pickup_point, object_to_pick_ind)) 00309 return -1; 00310 pickup_goal.target = processing_call.response.graspable_objects[object_to_pick_ind]; 00311 00312 //pass the name that the object has in the collision environment 00313 //this name was also returned by the collision map processor 00314 pickup_goal.collision_object_name = 00315 processing_call.response.collision_object_names.at(object_to_pick_ind); 00316 //pass the collision name of the table, also returned by the collision 00317 //map processor 00318 pickup_goal.collision_support_surface_name = 00319 processing_call.response.collision_support_surface_name; 00320 //pick up the object with the right arm 00321 pickup_goal.arm_name = "arm"; 00322 pickup_goal.allow_gripper_support_collision = true; 00323 //pickup_goal.desired_approach_distance = 0.06; 00324 //pickup_goal.min_approach_distance = 0.04; 00325 00326 //we will be lifting the object along the "vertical" direction 00327 //which is along the z axis in the base_link frame 00328 geometry_msgs::Vector3Stamped direction; 00329 direction.header.stamp = ros::Time::now(); 00330 direction.header.frame_id = "katana_base_link"; 00331 direction.vector.x = 0; 00332 direction.vector.y = 0; 00333 direction.vector.z = 1; 00334 pickup_goal.lift.direction = direction; 00335 //request a vertical lift of 10cm after grasping the object 00336 pickup_goal.lift.desired_distance = 0.1; 00337 pickup_goal.lift.min_distance = 0.05; 00338 //do not use tactile-based grasping or tactile-based lift 00339 pickup_goal.use_reactive_lift = false; 00340 pickup_goal.use_reactive_execution = false; 00341 //send the goal 00342 pickup_client.sendGoal(pickup_goal); 00343 while (!pickup_client.waitForResult(ros::Duration(10.0))) 00344 { 00345 ROS_INFO("Waiting for the pickup action..."); 00346 if (!nh.ok()) 00347 return -1; 00348 } 00349 object_manipulation_msgs::PickupResult pickup_result = *(pickup_client.getResult()); 00350 if (pickup_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 00351 { 00352 ROS_INFO("Pickup succeeded."); 00353 } else 00354 { 00355 ROS_ERROR("The pickup action has failed with result code %d", 00356 pickup_result.manipulation_result.value); 00357 return -1; 00358 } 00359 00360 //remember where we picked the object up from 00361 geometry_msgs::PoseStamped pickup_location; 00362 //for unrecognized point clouds, the location of the object is considered 00363 //to be the origin of the frame that the cluster is in 00364 pickup_location.header = 00365 processing_call.response.graspable_objects.at(object_to_pick_ind).cluster.header; 00366 //identity pose 00367 pickup_location.pose.orientation.w = 1; 00368 00369 //create a place location, offset by 30 cm from the pickup location 00370 geometry_msgs::PoseStamped place_location = pickup_location; 00371 place_location.header.stamp = ros::Time::now(); 00372 place_location.pose.position.y += 0.3; 00373 00374 // ----- put the object down 00375 ROS_INFO("Calling the place action"); 00376 object_manipulation_msgs::PlaceGoal place_goal; 00377 //place at the prepared location 00378 place_goal.place_locations.push_back(place_location); 00379 //the collision names of both the objects and the table 00380 //same as in the pickup action 00381 place_goal.collision_object_name = 00382 processing_call.response.collision_object_names.at(object_to_pick_ind); 00383 place_goal.collision_support_surface_name = 00384 processing_call.response.collision_support_surface_name; 00385 //information about which grasp was executed on the object, returned by 00386 //the pickup action 00387 place_goal.grasp = pickup_result.grasp; 00388 //use the arm to place 00389 place_goal.arm_name = "arm"; 00390 //padding used when determining if the requested place location 00391 //would bring the object in collision with the environment 00392 place_goal.place_padding = 0.02; 00393 //how much the gripper should retreat after placing the object 00394 place_goal.desired_retreat_distance = 0.1; 00395 place_goal.min_retreat_distance = 0.05; 00396 //we will be putting down the object along the "vertical" direction 00397 //which is along the z axis in the base_link frame 00398 direction.header.stamp = ros::Time::now(); 00399 direction.header.frame_id = "katana_base_link"; 00400 direction.vector.x = 0; 00401 direction.vector.y = 0; 00402 direction.vector.z = -1; 00403 place_goal.approach.direction = direction; 00404 //request a vertical put down motion of 10cm before placing the object 00405 place_goal.approach.desired_distance = 0.1; 00406 place_goal.approach.min_distance = 0.05; 00407 //we are not using tactile based placing 00408 place_goal.use_reactive_place = false; 00409 //send the goal 00410 place_client.sendGoal(place_goal); 00411 while (!place_client.waitForResult(ros::Duration(10.0))) 00412 { 00413 ROS_INFO("Waiting for the place action..."); 00414 if (!nh.ok()) 00415 return -1; 00416 } 00417 object_manipulation_msgs::PlaceResult place_result = 00418 *(place_client.getResult()); 00419 if (place_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) 00420 { 00421 ROS_ERROR("Place failed with error code %d", 00422 place_result.manipulation_result.value); 00423 return -1; 00424 } 00425 00426 // ----- move arm away again 00427 ROS_INFO("Moving arm away"); 00428 success = move_to_joint_goal(joint_constraints, move_arm); 00429 if (!success) 00430 return -1; 00431 00432 //success! 00433 ROS_INFO("Success! Object moved."); 00434 return 0; 00435 }