$search
00001 /********************************************************************* 00002 * 00003 * Copyright (c) 2009, Willow Garage, Inc. 00004 * All rights reserved. 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions 00008 * are met: 00009 * 00010 * * Redistributions of source code must retain the above copyright 00011 * notice, this list of conditions and the following disclaimer. 00012 * * Redistributions in binary form must reproduce the above 00013 * copyright notice, this list of conditions and the following 00014 * disclaimer in the documentation and/or other materials provided 00015 * with the distribution. 00016 * * Neither the name of the Willow Garage nor the names of its 00017 * contributors may be used to endorse or promote products derived 00018 * from this software without specific prior written permission. 00019 * 00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 * POSSIBILITY OF SUCH DAMAGE. 00032 *********************************************************************/ 00033 00034 #include <ros/ros.h> 00035 00036 //for getline 00037 #include <iostream> 00038 00039 //for termios 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 //for termios 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 //wait forever for connections 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 //print object names and tags 00219 ROS_INFO("Graspable objects:"); 00220 if (printObjects(objects, collision_names)) return -1; 00221 00222 //ask which one should be picked up 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 //for now, the cluster's reference frame is the frame of the object 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, 100); 00505 } 00506 catch (object_manipulator::GraspException &ex) 00507 { 00508 std::cout << ex.what() << std::endl; 00509 } 00510 } 00511 break; 00512 case 'r': 00513 try 00514 { 00515 collision_map_interface_.resetCollisionModels(); 00516 collision_map_interface_.resetAttachedModels(); 00517 } 00518 catch (tabletop_collision_map_processing::CollisionMapException &ex) 00519 { 00520 std::cout << ex.what() << std::endl; 00521 } 00522 break; 00523 case 'e': 00524 try 00525 { 00526 collision_map_interface_.resetAttachedModels(); 00527 } 00528 catch (object_manipulator::GraspException &ex) 00529 { 00530 std::cout << ex.what() << std::endl; 00531 } 00532 break; 00533 case 'm': 00534 { 00535 std::string arm_name = chooseArm(); 00536 std::vector<double> side_position(7, 0.0); 00537 if (arm_name=="right_arm") 00538 { 00539 side_position[0] = -2.135; 00540 side_position[1] = 0.803; 00541 side_position[2] = -1.732; 00542 side_position[3] = -1.905; 00543 side_position[4] = -2.369; 00544 side_position[5] = -1.680; 00545 side_position[6] = 1.398; 00546 } 00547 else 00548 { 00549 side_position[0] = 2.135; 00550 side_position[1] = 0.803; 00551 side_position[2] = 1.732; 00552 side_position[3] = -1.905; 00553 side_position[4] = 2.369; 00554 side_position[5] = -1.680; 00555 side_position[6] = 1.398; 00556 } 00557 try 00558 { 00559 arm_navigation_msgs::OrderedCollisionOperations empty_col; 00560 std::vector<arm_navigation_msgs::LinkPadding> empty_pad; 00561 if ( !mech_interface_.attemptMoveArmToGoal(arm_name, side_position, empty_col, empty_pad) ) 00562 std::cout << "Failed to move arm to side\n"; 00563 else 00564 std::cout << "Moved arm to side\n"; 00565 } 00566 catch (object_manipulator::MoveArmStuckException &ex) 00567 { 00568 std::cout << "Arm is stuck!\n"; 00569 } 00570 } 00571 break; 00572 case 'h': 00573 printOptions(); 00574 break; 00575 case 'q': 00576 return; 00577 case 'z': 00578 { 00579 std::string arm_name = chooseArm(); 00580 bool done = 0; 00581 while(nh_.ok() && !done) 00582 { 00583 char d; 00584 printMovementOptions(); 00585 if(read(kfd, &d, 1) < 0) 00586 { 00587 perror("read():"); 00588 exit(-1); 00589 } 00590 std::cout << "\n"; 00591 switch(d){ 00592 case 'u': 00593 translateGripperCartesian(arm_name, 2, .05); 00594 break; 00595 case 'd': 00596 translateGripperCartesian(arm_name, 2, -.05); 00597 break; 00598 case 'r': 00599 translateGripperCartesian(arm_name, 1, -.05); 00600 break; 00601 case 'l': 00602 translateGripperCartesian(arm_name, 1, .05); 00603 break; 00604 case 'f': 00605 translateGripperCartesian(arm_name, 0, .05); 00606 break; 00607 case 'b': 00608 translateGripperCartesian(arm_name, 0, -.05); 00609 break; 00610 case 'q': 00611 done = 1; 00612 break; 00613 } 00614 } 00615 break; 00616 } 00617 default: 00618 std::cout << "not a recognized option\n"; 00619 printOptions(); 00620 break; 00621 } 00622 } 00623 } 00624 }; 00625 00626 void quit(int sig) 00627 { 00628 tcsetattr(kfd, TCSANOW, &cooked); 00629 ros::shutdown(); 00630 exit(0); 00631 } 00632 00633 int main(int argc, char **argv) 00634 { 00635 signal(SIGINT,quit); 00636 00637 // get the console in raw mode 00638 tcgetattr(kfd, &cooked); 00639 memcpy(&raw, &cooked, sizeof(struct termios)); 00640 raw.c_lflag &=~ (ICANON | ECHO); 00641 // Setting a new line, then end of file 00642 raw.c_cc[VEOL] = 1; 00643 raw.c_cc[VEOF] = 2; 00644 tcsetattr(kfd, TCSANOW, &raw); 00645 00646 ros::init(argc, argv, "pick_and_place_keyboard_interface_node", ros::init_options::NoSigintHandler); 00647 ros::NodeHandle nh; 00648 PickAndPlaceKeyboardInterface node(nh); 00649 node.execute(); 00650 00651 tcsetattr(kfd, TCSANOW, &cooked); 00652 return 0; 00653 } 00654