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(bool take_new_map)
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.reset_static_map = take_new_map;
00318 processing_srv.request.take_static_collision_map = take_new_map;
00319 processing_srv.request.desired_frame = "base_link";
00320 if (!collision_processing_srv_.call(processing_srv))
00321 {
00322 ROS_ERROR("Collision map processing service failed");
00323 return;
00324 }
00325 objects_info_ = processing_srv.response;
00326 std::cout << "Detected " << objects_info_.graspable_objects.size() << " graspable object(s):\n";
00327 printObjects(objects_info_.graspable_objects, objects_info_.collision_object_names);
00328 }
00329
00330 void printOptions()
00331 {
00332 std::cout << "\n--Object manipulation options:\n";
00333 std::cout << "Use 'd' re-detect and process collision map, including taking a new static collision map\n";
00334 std::cout << "Use 't' re-detect and process collision map, without using a static collision map\n";
00335 std::cout << "Use 'p' to select an object and attempt to grasp it\n";
00336 std::cout << "Use 'w' to put the grasped object back down from where we took it\n";
00337 std::cout << "Use 'a' to transfer a grasped object to the other gripper\n";
00338 std::cout << "--Debug options:\n";
00339 std::cout << "Use 'o' to open gripper\n";
00340 std::cout << "Use 'c' to close gripper\n";
00341 std::cout << "Use 'r' to reset collision map\n";
00342 std::cout << "Use 's' to take a static collision map\n";
00343 std::cout << "Use 'e' to detach all objects from gripper\n";
00344 std::cout << "Use 'm' to move arm to side\n";
00345 std::cout << "Use 'z' for Cartesian movement\n";
00346 std::cout << "--Misc options:\n";
00347 std::cout << "Use 'h' to get this menu\n";
00348 std::cout << "Use 'q' to quit\n";
00349 }
00350
00351 void printMovementOptions()
00352 {
00353 std::cout << "Use 'u' and 'd' to move up or down 5 cm\n";
00354 std::cout << "Use 'l' and 'r' to move left or right 5 cm\n";
00355 std::cout << "Use 'f' and 'b' to move forward or back 5 cm\n";
00356 std::cout << "Use 'q' to quit back to the main menu\n";
00357 }
00358
00359 void translateGripperCartesian(std::string arm_name, int axis, double dist)
00360 {
00361 geometry_msgs::Vector3Stamped direction;
00362 direction.header.frame_id = "base_link";
00363 direction.header.stamp = ros::Time::now();
00364 if(axis == 0) direction.vector.x = dist;
00365 else if(axis == 1) direction.vector.y = dist;
00366 else if(axis == 2) direction.vector.z = dist;
00367 else return;
00368 std::cout << "desired direction:" << direction;
00369 mech_interface_.translateGripperCartesian(arm_name, direction, ros::Duration(2.0), .015, .09, .02, .16, .1);
00370 }
00371
00372 void execute()
00373 {
00374 char c;
00375 while(nh_.ok())
00376 {
00377 printOptions();
00378 std::cout << ">> \n";
00379 if(read(kfd, &c, 1) < 0)
00380 {
00381 perror("read():");
00382 exit(-1);
00383 }
00384 std::cout << "\n";
00385 switch(c){
00386 case 'd':
00387 detectAndProcessCollisionMap(true);
00388 break;
00389 case 't':
00390 detectAndProcessCollisionMap(false);
00391 break;
00392 case 'p':
00393 {
00394 if(objects_info_.graspable_objects.empty())
00395 {
00396 std::cout << "No graspable objects available. Detect again.\n";
00397 break;
00398 }
00399 int pickup = chooseObject(objects_info_.graspable_objects, objects_info_.collision_object_names);
00400 if (pickup < 0 || pickup >= (int)objects_info_.graspable_objects.size())
00401 {
00402 std::cout << "Not an allowable value for object selection\n";
00403 break;
00404 }
00405 object_manipulation_msgs::PickupGoal goal;
00406 goal.target = objects_info_.graspable_objects.at(pickup);
00407 goal.arm_name = chooseArm();
00408 goal.collision_object_name = objects_info_.collision_object_names.at(pickup);
00409 goal.collision_support_surface_name = objects_info_.collision_support_surface_name;
00410
00411 goal.desired_approach_distance = 0.1;
00412 goal.min_approach_distance = 0.05;
00413 goal.lift.direction = chooseDirection(goal.arm_name, true);
00414 goal.lift.desired_distance = 0.1;
00415 goal.lift.min_distance = 0.05;
00416
00417 goal.use_reactive_lift = true;
00418 goal.use_reactive_execution = true;
00419 if (!objects_info_.graspable_objects.at(pickup).potential_models.empty())
00420 {
00421 goal.use_reactive_lift = false;
00422 goal.use_reactive_execution = false;
00423 }
00424 pickup_client_.sendGoal(goal);
00425 while (!pickup_client_.waitForResult(ros::Duration(10.0)))
00426 {
00427 ROS_INFO("Waiting for the pickup action...");
00428 }
00429
00430 object_manipulation_msgs::PickupResult result = *(pickup_client_.getResult());
00431 if (pickup_client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00432 {
00433 std::cout << "The pickup action has failed with result code " << result.manipulation_result.value << "\n";
00434 break;
00435 }
00436
00437 std::cout << "Pickup reports success\n";
00438 geometry_msgs::PoseStamped pickup_location;
00439
00440 pickup_location.header = objects_info_.graspable_objects.at(pickup).cluster.header;
00441 pickup_location.pose.position.x = pickup_location.pose.position.y = pickup_location.pose.position.z = 0;
00442 pickup_location.pose.orientation.x = pickup_location.pose.orientation.y =
00443 pickup_location.pose.orientation.z = 0;
00444 pickup_location.pose.orientation.w = 1;
00445
00446 GraspInfo grasp_info;
00447 grasp_info.pickup_location_ = pickup_location;
00448 grasp_info.grasp_ = result.grasp;
00449 grasp_info.collision_object_name_ = goal.collision_object_name;
00450
00451 if (goal.arm_name == "right_arm") grasp_info_right_ = grasp_info;
00452 else grasp_info_left_ = grasp_info;
00453 }
00454 break;
00455 case 'w':
00456 {
00457 object_manipulation_msgs::PlaceGoal goal;
00458 goal.arm_name = chooseArm();
00459 goal.place_padding = 0.02;
00460 goal.collision_support_surface_name = objects_info_.collision_support_surface_name;
00461 goal.use_reactive_place = true;
00462
00463 GraspInfo grasp_info = grasp_info_left_;
00464 if (goal.arm_name == "right_arm") grasp_info = grasp_info_right_;
00465 goal.grasp = grasp_info.grasp_;
00466 goal.place_locations.push_back(grasp_info.pickup_location_);
00467 goal.collision_object_name = grasp_info.collision_object_name_;
00468
00469 goal.desired_retreat_distance = 0.1;
00470 goal.min_retreat_distance = 0.05;
00471 goal.approach.direction = chooseDirection(goal.arm_name, false);
00472 goal.approach.desired_distance = 0.1;
00473 goal.approach.min_distance = 0.05;
00474
00475 place_client_.sendGoal(goal);
00476 while (!place_client_.waitForResult(ros::Duration(10.0)))
00477 {
00478 ROS_INFO("Waiting for the place action...");
00479 }
00480 object_manipulation_msgs::PlaceResult result = *(place_client_.getResult());
00481 if (place_client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00482 {
00483 std::cout << "Place failed with error code " << result.manipulation_result.value << "\n";
00484 break;
00485 }
00486 std::cout << "Object placed successfully\n";
00487 }
00488 break;
00489 case 'o':
00490 {
00491 std::string arm_name = chooseArm();
00492 object_manipulation_msgs::Grasp grasp;
00493 try
00494 {
00495 mech_interface_.handPostureGraspAction(arm_name, grasp,
00496 object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE);
00497 }
00498 catch (object_manipulator::GraspException &ex)
00499 {
00500 std::cout << ex.what() << std::endl;
00501 }
00502 }
00503 break;
00504 case 'c':
00505 {
00506 std::string arm_name = chooseArm();
00507 object_manipulation_msgs::Grasp grasp;
00508 grasp.grasp_posture.position.push_back(0.0);
00509 grasp.grasp_posture.effort.push_back(10000);
00510 try
00511 {
00512 mech_interface_.handPostureGraspAction(arm_name, grasp,
00513 object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP);
00514 }
00515 catch (object_manipulator::GraspException &ex)
00516 {
00517 std::cout << ex.what() << std::endl;
00518 }
00519 }
00520 break;
00521 case 'r':
00522 try
00523 {
00524 collision_map_interface_.resetCollisionModels();
00525 collision_map_interface_.resetAttachedModels();
00526 }
00527 catch (tabletop_collision_map_processing::CollisionMapException &ex)
00528 {
00529 std::cout << ex.what() << std::endl;
00530 }
00531 break;
00532 case 's':
00533 try
00534 {
00535 collision_map_interface_.takeStaticMap();
00536 }
00537 catch (tabletop_collision_map_processing::CollisionMapException &ex)
00538 {
00539 std::cout << ex.what() << std::endl;
00540 }
00541 break;
00542 case 'e':
00543 try
00544 {
00545 collision_map_interface_.resetAttachedModels();
00546 }
00547 catch (object_manipulator::GraspException &ex)
00548 {
00549 std::cout << ex.what() << std::endl;
00550 }
00551 break;
00552 case 'm':
00553 {
00554 std::string arm_name = chooseArm();
00555 std::vector<double> side_position(7, 0.0);
00556 if (arm_name=="right_arm")
00557 {
00558 side_position[0] = -2.135;
00559 side_position[1] = 0.803;
00560 side_position[2] = -1.732;
00561 side_position[3] = -1.905;
00562 side_position[4] = -2.369;
00563 side_position[5] = -1.680;
00564 side_position[6] = 1.398;
00565 }
00566 else
00567 {
00568 side_position[0] = 2.135;
00569 side_position[1] = 0.803;
00570 side_position[2] = 1.732;
00571 side_position[3] = -1.905;
00572 side_position[4] = 2.369;
00573 side_position[5] = -1.680;
00574 side_position[6] = 1.398;
00575 }
00576 try
00577 {
00578 motion_planning_msgs::OrderedCollisionOperations empty_col;
00579 std::vector<motion_planning_msgs::LinkPadding> empty_pad;
00580 if ( !mech_interface_.attemptMoveArmToGoal(arm_name, side_position, empty_col, empty_pad) )
00581 std::cout << "Failed to move arm to side\n";
00582 else
00583 std::cout << "Moved arm to side\n";
00584 }
00585 catch (object_manipulator::MoveArmStuckException &ex)
00586 {
00587 std::cout << "Arm is stuck!\n";
00588 }
00589 }
00590 break;
00591 case 'h':
00592 printOptions();
00593 break;
00594 case 'q':
00595 return;
00596 case 'z':
00597 {
00598 std::string arm_name = chooseArm();
00599 bool done = 0;
00600 while(nh_.ok() && !done)
00601 {
00602 char d;
00603 printMovementOptions();
00604 if(read(kfd, &d, 1) < 0)
00605 {
00606 perror("read():");
00607 exit(-1);
00608 }
00609 std::cout << "\n";
00610 switch(d){
00611 case 'u':
00612 translateGripperCartesian(arm_name, 2, .05);
00613 break;
00614 case 'd':
00615 translateGripperCartesian(arm_name, 2, -.05);
00616 break;
00617 case 'r':
00618 translateGripperCartesian(arm_name, 1, -.05);
00619 break;
00620 case 'l':
00621 translateGripperCartesian(arm_name, 1, .05);
00622 break;
00623 case 'f':
00624 translateGripperCartesian(arm_name, 0, .05);
00625 break;
00626 case 'b':
00627 translateGripperCartesian(arm_name, 0, -.05);
00628 break;
00629 case 'q':
00630 done = 1;
00631 break;
00632 }
00633 }
00634 break;
00635 }
00636 default:
00637 std::cout << "not a recognized option\n";
00638 printOptions();
00639 break;
00640 }
00641 }
00642 }
00643 };
00644
00645 void quit(int sig)
00646 {
00647 tcsetattr(kfd, TCSANOW, &cooked);
00648 ros::shutdown();
00649 exit(0);
00650 }
00651
00652 int main(int argc, char **argv)
00653 {
00654 signal(SIGINT,quit);
00655
00656
00657 tcgetattr(kfd, &cooked);
00658 memcpy(&raw, &cooked, sizeof(struct termios));
00659 raw.c_lflag &=~ (ICANON | ECHO);
00660
00661 raw.c_cc[VEOL] = 1;
00662 raw.c_cc[VEOF] = 2;
00663 tcsetattr(kfd, TCSANOW, &raw);
00664
00665 ros::init(argc, argv, "pick_and_place_keyboard_interface_node", ros::init_options::NoSigintHandler);
00666 ros::NodeHandle nh;
00667 PickAndPlaceKeyboardInterface node(nh);
00668 node.execute();
00669
00670 tcsetattr(kfd, TCSANOW, &cooked);
00671 return 0;
00672 }
00673