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
00031
00032
00033
00034 #include <ros/ros.h>
00035
00036
00037 #include <iostream>
00038
00039
00040 #include <termios.h>
00041 #include <signal.h>
00042 #include <cstdio>
00043 #include <cstdlib>
00044 #include <unistd.h>
00045 #include <math.h>
00046
00047 #include <actionlib/client/simple_action_client.h>
00048
00049 #include <household_objects_database_msgs/GetModelDescription.h>
00050
00051 #include <object_manipulator/tools/mechanism_interface.h>
00052
00053 #include <tabletop_object_detector/TabletopDetection.h>
00054
00055 #include <tabletop_collision_map_processing/collision_map_interface.h>
00056 #include <tabletop_collision_map_processing/TabletopCollisionMapProcessing.h>
00057
00058 #include <object_manipulation_msgs/PickupAction.h>
00059 #include <object_manipulation_msgs/PlaceAction.h>
00060
00061 static const std::string OBJECT_DETECTION_SERVICE_NAME = "/object_detection";
00062 static const std::string COLLISION_PROCESSING_SERVICE_NAME =
00063 "/tabletop_collision_map_processing/tabletop_collision_map_processing";
00064
00065 static const std::string PICKUP_ACTION_NAME = "/object_manipulator/object_manipulator_pickup";
00066 static const std::string PLACE_ACTION_NAME = "/object_manipulator/object_manipulator_place";
00067 static const std::string GET_MODEL_DESCRIPTION_NAME = "/objects_database_node/get_model_description";
00068
00069
00070 int kfd = 0;
00071 struct termios cooked, raw;
00072
00073 class PickAndPlaceKeyboardInterface
00074 {
00075 private:
00076 ros::NodeHandle nh_;
00077 ros::NodeHandle priv_nh_;
00078
00079 ros::ServiceClient object_detection_srv_;
00080 ros::ServiceClient collision_processing_srv_;
00081 ros::ServiceClient get_model_description_srv_;
00082
00083 actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> pickup_client_;
00084 actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> place_client_;
00085
00086 object_manipulator::MechanismInterface mech_interface_;
00087 tabletop_collision_map_processing::CollisionMapInterface collision_map_interface_;
00088
00089 struct GraspInfo
00090 {
00091 geometry_msgs::PoseStamped pickup_location_;
00092 object_manipulation_msgs::Grasp grasp_;
00093 std::string collision_object_name_;
00094 };
00095 GraspInfo grasp_info_right_;
00096 GraspInfo grasp_info_left_;
00097
00098 tabletop_collision_map_processing::TabletopCollisionMapProcessing::Response objects_info_;
00099
00100 public:
00101 PickAndPlaceKeyboardInterface(ros::NodeHandle &nh) :
00102 nh_(nh),
00103 priv_nh_("~"),
00104 pickup_client_(PICKUP_ACTION_NAME, true),
00105 place_client_(PLACE_ACTION_NAME, true),
00106 mech_interface_()
00107 {
00108
00109 collision_map_interface_.connectionsEstablished(ros::Duration(-1.0));
00110
00111 while ( !ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME, ros::Duration(2.0)) && nh_.ok() )
00112 {
00113 ROS_INFO("Waiting for object detection service to come up");
00114 }
00115 if (!nh_.ok()) exit(0);
00116 object_detection_srv_ = nh_.serviceClient<tabletop_object_detector::TabletopDetection>
00117 (OBJECT_DETECTION_SERVICE_NAME, true);
00118
00119 while ( !ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME, ros::Duration(2.0)) && nh_.ok() )
00120 {
00121 ROS_INFO("Waiting for collision processing service to come up");
00122 }
00123 if (!nh_.ok()) exit(0);
00124 collision_processing_srv_ = nh_.serviceClient<tabletop_collision_map_processing::TabletopCollisionMapProcessing>
00125 (COLLISION_PROCESSING_SERVICE_NAME, true);
00126
00127 while(!pickup_client_.waitForServer(ros::Duration(2.0)) && priv_nh_.ok())
00128 {
00129 ROS_INFO_STREAM("Waiting for action client " << PICKUP_ACTION_NAME);
00130 }
00131 if (!priv_nh_.ok()) exit(0);
00132
00133 while(!place_client_.waitForServer(ros::Duration(2.0)) && priv_nh_.ok())
00134 {
00135 ROS_INFO_STREAM("Waiting for action client " << PLACE_ACTION_NAME);
00136 }
00137 if (!priv_nh_.ok()) exit(0);
00138
00139 bool use_database;
00140 priv_nh_.param<bool>("use_database", use_database, false);
00141
00142 if (use_database)
00143 {
00144 while ( !ros::service::waitForService(GET_MODEL_DESCRIPTION_NAME, ros::Duration(2.0)) && nh_.ok() )
00145 {
00146 ROS_INFO("Waiting for %s service to come up", GET_MODEL_DESCRIPTION_NAME.c_str());
00147 }
00148 if (!nh_.ok()) exit(0);
00149 get_model_description_srv_ = nh_.serviceClient<household_objects_database_msgs::GetModelDescription>
00150 (GET_MODEL_DESCRIPTION_NAME, true);
00151 }
00152 }
00153
00154 ~PickAndPlaceKeyboardInterface() {}
00155
00156 bool getModelInfo(const household_objects_database_msgs::DatabaseModelPose &model_pose,
00157 std::string &name, std::string &all_tags)
00158 {
00159 household_objects_database_msgs::GetModelDescription desc;
00160 desc.request.model_id = model_pose.model_id;
00161 if ( !get_model_description_srv_.call(desc) )
00162 {
00163 ROS_ERROR("Failed to call get model description service");
00164 return false;
00165 }
00166
00167 if (desc.response.return_code.code != desc.response.return_code.SUCCESS )
00168 {
00169 ROS_ERROR("Model description service reports an error (code %d)", desc.response.return_code.code);
00170 return false;
00171 }
00172 name = desc.response.name;
00173 for (size_t i=0; i<desc.response.tags.size(); i++)
00174 {
00175 if (!all_tags.empty()) all_tags.append(",");
00176 all_tags.append(desc.response.tags.at(i));
00177 }
00178 return true;
00179 }
00180
00181 int printObjects(const std::vector<object_manipulation_msgs::GraspableObject> &objects,
00182 const std::vector<std::string> &collision_names)
00183 {
00184 for (size_t m=0; m<objects.size(); m++)
00185 {
00186 std::string name, all_tags;
00187 std::string collision_name = collision_names.at(m);
00188 if (!objects[m].potential_models.empty())
00189 {
00190 if (getModelInfo(objects[m].potential_models[0], name, all_tags))
00191 {
00192 ROS_INFO(" (%d): %s (%s) in frame %s (%s)", (int)m, name.c_str(), all_tags.c_str(),
00193 objects[m].potential_models[0].pose.header.frame_id.c_str(), collision_name.c_str());
00194 }
00195 else
00196 {
00197 ROS_INFO(" (%d): database object, details not available (%s)", (int)m,
00198 collision_name.c_str());
00199 }
00200 }
00201 else
00202 {
00203 ROS_INFO(" (%d): unrecognized cluster with %d points (%s)", (int)m,
00204 (unsigned int)objects[m].cluster.points.size(), collision_name.c_str());
00205 }
00206 }
00207 return 0;
00208 }
00209
00210 int chooseObject(const std::vector<object_manipulation_msgs::GraspableObject> &objects,
00211 const std::vector<std::string> &collision_names)
00212 {
00213 if (objects.empty())
00214 {
00215 ROS_INFO("Detection result contains no graspable objects");
00216 return -1;
00217 }
00218
00219 ROS_INFO("Graspable objects:");
00220 if (printObjects(objects, collision_names)) return -1;
00221
00222
00223 char c;
00224 ROS_INFO("Enter number of object for selection:");
00225 if(read(kfd, &c, 1) < 0)
00226 {
00227 perror("read():");
00228 exit(-1);
00229 }
00230 int sel;
00231 sel = atoi(&c);
00232 std::cout << "Selected " << sel << std::endl;
00233 if (sel < 0 || sel >= (int)objects.size())
00234 {
00235 ROS_INFO("No object selected");
00236 return -1;
00237 }
00238 return sel;
00239 }
00240
00241 geometry_msgs::Vector3Stamped chooseDirection(std::string arm_name, bool grasp)
00242 {
00243 char c;
00244 while (1)
00245 {
00246 std::cout << " press 'u' for lift or place along up/down direction\n";
00247 std::cout << " press 'g' for lift or place along gripper direction\n";
00248 if(read(kfd, &c, 1) < 0)
00249 {
00250 perror("read():");
00251 exit(-1);
00252 }
00253 std::cout << "\n";
00254 if ( c=='u' || c=='g') break;
00255 else std::cout << " invalid selection\n";
00256 }
00257 geometry_msgs::Vector3Stamped direction;
00258 direction.header.stamp = ros::Time::now();
00259 if ( c=='u' )
00260 {
00261 direction.header.frame_id = "base_link";
00262 direction.vector.x = 0;
00263 direction.vector.y = 0;
00264 direction.vector.z = 1;
00265 if (!grasp) direction.vector.z = -1;
00266 }
00267 else
00268 {
00269 if (arm_name == "right_arm") direction.header.frame_id = "r_wrist_roll_link";
00270 else direction.header.frame_id = "l_wrist_roll_link";
00271 direction.vector.x = -1;
00272 direction.vector.y = 0;
00273 direction.vector.z = 0;
00274 if (!grasp) direction.vector.x = 1;
00275 }
00276 return direction;
00277 }
00278
00279 std::string chooseArm()
00280 {
00281 bool done = false;
00282 char c;
00283 while ( !done )
00284 {
00285 std::cout << " press 'r' for right arm\n";
00286 std::cout << " press 'l' for left arm\n";
00287 if(read(kfd, &c, 1) < 0)
00288 {
00289 perror("read():");
00290 exit(-1);
00291 }
00292 std::cout << "\n";
00293 if ( c=='r' || c=='l') done = true;
00294 else std::cout << " invalid selection\n";
00295 }
00296 if (c=='l') return "left_arm";
00297 return "right_arm";
00298 }
00299
00300 void detectAndProcessCollisionMap()
00301 {
00302 ROS_INFO("Calling detection service");
00303 tabletop_object_detector::TabletopDetection detection_srv;
00304 detection_srv.request.return_clusters = true;
00305 detection_srv.request.return_models = true;
00306 detection_srv.request.num_models = 5;
00307 if (!object_detection_srv_.call(detection_srv))
00308 {
00309 ROS_ERROR("Tabletop detection service failed");
00310 return;
00311 }
00312 ROS_INFO("Detection complete; calling collision map processing");
00313 tabletop_collision_map_processing::TabletopCollisionMapProcessing processing_srv;
00314 processing_srv.request.detection_result = detection_srv.response.detection;
00315 processing_srv.request.reset_collision_models = true;
00316 processing_srv.request.reset_attached_models = true;
00317 processing_srv.request.desired_frame = "base_link";
00318 if (!collision_processing_srv_.call(processing_srv))
00319 {
00320 ROS_ERROR("Collision map processing service failed");
00321 return;
00322 }
00323 objects_info_ = processing_srv.response;
00324 std::cout << "Detected " << objects_info_.graspable_objects.size() << " graspable object(s):\n";
00325 printObjects(objects_info_.graspable_objects, objects_info_.collision_object_names);
00326 }
00327
00328 void printOptions()
00329 {
00330 std::cout << "\n--Object manipulation options:\n";
00331 std::cout << "Use 'd' re-detect and process collision map\n";
00332 std::cout << "Use 'p' to select an object and attempt to grasp it\n";
00333 std::cout << "Use 'w' to put the grasped object back down from where we took it\n";
00334 std::cout << "Use 'a' to transfer a grasped object to the other gripper\n";
00335 std::cout << "--Debug options:\n";
00336 std::cout << "Use 'o' to open gripper\n";
00337 std::cout << "Use 'c' to close gripper\n";
00338 std::cout << "Use 'r' to reset collision map\n";
00339 std::cout << "Use 'e' to detach all objects from gripper\n";
00340 std::cout << "Use 'm' to move arm to side\n";
00341 std::cout << "Use 'z' for Cartesian movement\n";
00342 std::cout << "--Misc options:\n";
00343 std::cout << "Use 'h' to get this menu\n";
00344 std::cout << "Use 'q' to quit\n";
00345 }
00346
00347 void printMovementOptions()
00348 {
00349 std::cout << "Use 'u' and 'd' to move up or down 5 cm\n";
00350 std::cout << "Use 'l' and 'r' to move left or right 5 cm\n";
00351 std::cout << "Use 'f' and 'b' to move forward or back 5 cm\n";
00352 std::cout << "Use 'q' to quit back to the main menu\n";
00353 }
00354
00355 void translateGripperCartesian(std::string arm_name, int axis, double dist)
00356 {
00357 geometry_msgs::Vector3Stamped direction;
00358 direction.header.frame_id = "base_link";
00359 direction.header.stamp = ros::Time::now();
00360 if(axis == 0) direction.vector.x = dist;
00361 else if(axis == 1) direction.vector.y = dist;
00362 else if(axis == 2) direction.vector.z = dist;
00363 else return;
00364 std::cout << "desired direction:" << direction;
00365 mech_interface_.translateGripperCartesian(arm_name, direction, ros::Duration(2.0), .015, .09, .02, .16, .1);
00366 }
00367
00368 void execute()
00369 {
00370 char c;
00371 while(nh_.ok())
00372 {
00373 printOptions();
00374 std::cout << ">> \n";
00375 if(read(kfd, &c, 1) < 0)
00376 {
00377 perror("read():");
00378 exit(-1);
00379 }
00380 std::cout << "\n";
00381 switch(c){
00382 case 'd':
00383 detectAndProcessCollisionMap();
00384 break;
00385 case 'p':
00386 {
00387 if(objects_info_.graspable_objects.empty())
00388 {
00389 std::cout << "No graspable objects available. Detect again.\n";
00390 break;
00391 }
00392 int pickup = chooseObject(objects_info_.graspable_objects, objects_info_.collision_object_names);
00393 if (pickup < 0 || pickup >= (int)objects_info_.graspable_objects.size())
00394 {
00395 std::cout << "Not an allowable value for object selection\n";
00396 break;
00397 }
00398 object_manipulation_msgs::PickupGoal goal;
00399 goal.target = objects_info_.graspable_objects.at(pickup);
00400 goal.arm_name = chooseArm();
00401 goal.collision_object_name = objects_info_.collision_object_names.at(pickup);
00402 goal.collision_support_surface_name = objects_info_.collision_support_surface_name;
00403
00404 goal.lift.direction = chooseDirection(goal.arm_name, true);
00405 goal.lift.desired_distance = 0.1;
00406 goal.lift.min_distance = 0.05;
00407
00408 goal.use_reactive_lift = true;
00409 goal.use_reactive_execution = true;
00410 if (!objects_info_.graspable_objects.at(pickup).potential_models.empty())
00411 {
00412 goal.use_reactive_lift = false;
00413 goal.use_reactive_execution = false;
00414 }
00415 pickup_client_.sendGoal(goal);
00416 while (!pickup_client_.waitForResult(ros::Duration(10.0)))
00417 {
00418 ROS_INFO("Waiting for the pickup action...");
00419 }
00420
00421 object_manipulation_msgs::PickupResult result = *(pickup_client_.getResult());
00422 if (pickup_client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00423 {
00424 std::cout << "The pickup action has failed with result code " << result.manipulation_result.value << "\n";
00425 break;
00426 }
00427
00428 std::cout << "Pickup reports success\n";
00429 geometry_msgs::PoseStamped pickup_location;
00430
00431 pickup_location.header = objects_info_.graspable_objects.at(pickup).cluster.header;
00432 pickup_location.pose.position.x = pickup_location.pose.position.y = pickup_location.pose.position.z = 0;
00433 pickup_location.pose.orientation.x = pickup_location.pose.orientation.y =
00434 pickup_location.pose.orientation.z = 0;
00435 pickup_location.pose.orientation.w = 1;
00436
00437 GraspInfo grasp_info;
00438 grasp_info.pickup_location_ = pickup_location;
00439 grasp_info.grasp_ = result.grasp;
00440 grasp_info.collision_object_name_ = goal.collision_object_name;
00441
00442 if (goal.arm_name == "right_arm") grasp_info_right_ = grasp_info;
00443 else grasp_info_left_ = grasp_info;
00444 }
00445 break;
00446 case 'w':
00447 {
00448 object_manipulation_msgs::PlaceGoal goal;
00449 goal.arm_name = chooseArm();
00450 goal.place_padding = 0.02;
00451 goal.collision_support_surface_name = objects_info_.collision_support_surface_name;
00452 goal.use_reactive_place = true;
00453
00454 GraspInfo grasp_info = grasp_info_left_;
00455 if (goal.arm_name == "right_arm") grasp_info = grasp_info_right_;
00456 goal.grasp = grasp_info.grasp_;
00457 goal.place_locations.push_back(grasp_info.pickup_location_);
00458 goal.collision_object_name = grasp_info.collision_object_name_;
00459
00460 goal.desired_retreat_distance = 0.1;
00461 goal.min_retreat_distance = 0.05;
00462 goal.approach.direction = chooseDirection(goal.arm_name, false);
00463 goal.approach.desired_distance = 0.1;
00464 goal.approach.min_distance = 0.05;
00465
00466 place_client_.sendGoal(goal);
00467 while (!place_client_.waitForResult(ros::Duration(10.0)))
00468 {
00469 ROS_INFO("Waiting for the place action...");
00470 }
00471 object_manipulation_msgs::PlaceResult result = *(place_client_.getResult());
00472 if (place_client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00473 {
00474 std::cout << "Place failed with error code " << result.manipulation_result.value << "\n";
00475 break;
00476 }
00477 std::cout << "Object placed successfully\n";
00478 }
00479 break;
00480 case 'o':
00481 {
00482 std::string arm_name = chooseArm();
00483 object_manipulation_msgs::Grasp grasp;
00484 try
00485 {
00486 mech_interface_.handPostureGraspAction(arm_name, grasp,
00487 object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE, 100);
00488 }
00489 catch (object_manipulator::GraspException &ex)
00490 {
00491 std::cout << ex.what() << std::endl;
00492 }
00493 }
00494 break;
00495 case 'c':
00496 {
00497 std::string arm_name = chooseArm();
00498 object_manipulation_msgs::Grasp grasp;
00499 grasp.grasp_posture.position.push_back(0.0);
00500 grasp.grasp_posture.effort.push_back(10000);
00501 try
00502 {
00503 mech_interface_.handPostureGraspAction(arm_name, grasp,
00504 object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP, 50);
00505
00506 }
00507 catch (object_manipulator::GraspException &ex)
00508 {
00509 std::cout << ex.what() << std::endl;
00510 }
00511 }
00512 break;
00513 case 'r':
00514 try
00515 {
00516 collision_map_interface_.resetCollisionModels();
00517 collision_map_interface_.resetAttachedModels();
00518 }
00519 catch (tabletop_collision_map_processing::CollisionMapException &ex)
00520 {
00521 std::cout << ex.what() << std::endl;
00522 }
00523 break;
00524 case 'e':
00525 try
00526 {
00527 collision_map_interface_.resetAttachedModels();
00528 }
00529 catch (object_manipulator::GraspException &ex)
00530 {
00531 std::cout << ex.what() << std::endl;
00532 }
00533 break;
00534 case 'm':
00535 {
00536 std::string arm_name = chooseArm();
00537 std::vector<double> side_position(7, 0.0);
00538 if (arm_name=="right_arm")
00539 {
00540 side_position[0] = -2.135;
00541 side_position[1] = 0.803;
00542 side_position[2] = -1.732;
00543 side_position[3] = -1.905;
00544 side_position[4] = -2.369;
00545 side_position[5] = -1.680;
00546 side_position[6] = 1.398;
00547 }
00548 else
00549 {
00550 side_position[0] = 2.135;
00551 side_position[1] = 0.803;
00552 side_position[2] = 1.732;
00553 side_position[3] = -1.905;
00554 side_position[4] = 2.369;
00555 side_position[5] = -1.680;
00556 side_position[6] = 1.398;
00557 }
00558 try
00559 {
00560 arm_navigation_msgs::OrderedCollisionOperations empty_col;
00561 std::vector<arm_navigation_msgs::LinkPadding> empty_pad;
00562 if ( !mech_interface_.attemptMoveArmToGoal(arm_name, side_position, empty_col, empty_pad) )
00563 std::cout << "Failed to move arm to side\n";
00564 else
00565 std::cout << "Moved arm to side\n";
00566 }
00567 catch (object_manipulator::MoveArmStuckException &ex)
00568 {
00569 std::cout << "Arm is stuck!\n";
00570 }
00571 }
00572 break;
00573 case 'h':
00574 printOptions();
00575 break;
00576 case 'q':
00577 return;
00578 case 'z':
00579 {
00580 std::string arm_name = chooseArm();
00581 bool done = 0;
00582 while(nh_.ok() && !done)
00583 {
00584 char d;
00585 printMovementOptions();
00586 if(read(kfd, &d, 1) < 0)
00587 {
00588 perror("read():");
00589 exit(-1);
00590 }
00591 std::cout << "\n";
00592 switch(d){
00593 case 'u':
00594 translateGripperCartesian(arm_name, 2, .05);
00595 break;
00596 case 'd':
00597 translateGripperCartesian(arm_name, 2, -.05);
00598 break;
00599 case 'r':
00600 translateGripperCartesian(arm_name, 1, -.05);
00601 break;
00602 case 'l':
00603 translateGripperCartesian(arm_name, 1, .05);
00604 break;
00605 case 'f':
00606 translateGripperCartesian(arm_name, 0, .05);
00607 break;
00608 case 'b':
00609 translateGripperCartesian(arm_name, 0, -.05);
00610 break;
00611 case 'q':
00612 done = 1;
00613 break;
00614 }
00615 }
00616 break;
00617 }
00618 default:
00619 std::cout << "not a recognized option\n";
00620 printOptions();
00621 break;
00622 }
00623 }
00624 }
00625 };
00626
00627 void quit(int sig)
00628 {
00629 tcsetattr(kfd, TCSANOW, &cooked);
00630 ros::shutdown();
00631 exit(0);
00632 }
00633
00634 int main(int argc, char **argv)
00635 {
00636 signal(SIGINT,quit);
00637
00638
00639 tcgetattr(kfd, &cooked);
00640 memcpy(&raw, &cooked, sizeof(struct termios));
00641 raw.c_lflag &=~ (ICANON | ECHO);
00642
00643 raw.c_cc[VEOL] = 1;
00644 raw.c_cc[VEOF] = 2;
00645 tcsetattr(kfd, TCSANOW, &raw);
00646
00647 ros::init(argc, argv, "pick_and_place_keyboard_interface_node", ros::init_options::NoSigintHandler);
00648 ros::NodeHandle nh;
00649 PickAndPlaceKeyboardInterface node(nh);
00650 node.execute();
00651
00652 tcsetattr(kfd, TCSANOW, &cooked);
00653 return 0;
00654 }
00655