$search
00001 /* 00002 * pr2_teleop_general_joystick 00003 * Copyright (c) 2010, 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 are met: 00008 * 00009 * * Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * * Redistributions in binary form must reproduce the above copyright 00012 * notice, this list of conditions and the following disclaimer in the 00013 * documentation and/or other materials provided with the distribution. 00014 * * Neither the name of the <ORGANIZATION> nor the names of its 00015 * contributors may be used to endorse or promote products derived from 00016 * this software without specific prior written permission. 00017 * 00018 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 * POSSIBILITY OF SUCH DAMAGE. 00029 */ 00030 00031 // Author: E. Gil Jones 00032 00033 #include <termios.h> 00034 #include <signal.h> 00035 #include <cstdio> 00036 #include <cstdlib> 00037 #include <unistd.h> 00038 #include <math.h> 00039 00040 #include <ros/ros.h> 00041 00042 #include <sensor_msgs/Joy.h> 00043 #include "pr2_teleop_general/pr2_teleop_general_commander.h" 00044 00045 enum JoystickLayoutMode { 00046 LAYOUT_NONE, 00047 LAYOUT_BODY, 00048 LAYOUT_HEAD, 00049 LAYOUT_RIGHT_ARM, 00050 LAYOUT_LEFT_ARM, 00051 LAYOUT_BOTH_ARMS 00052 }; 00053 00054 static const unsigned int BODY_LAYOUT_BUTTON = 10; 00055 static const unsigned int RIGHT_ARM_LAYOUT_BUTTON = 9; 00056 static const unsigned int LEFT_ARM_LAYOUT_BUTTON = 8; 00057 static const unsigned int HEAD_LAYOUT_BUTTON = 11; 00058 00059 static const unsigned int HEAD_MODE_TOGGLE_BUTTON = 4; 00060 static const unsigned int PROJECTOR_TOGGLE_BUTTON = 5; 00061 static const unsigned int LASER_TOGGLE_BUTTON = 7; 00062 static const unsigned int PROSILICA_POLL_BUTTON = 6; 00063 static const unsigned int OPEN_GRIPPER_BUTTON = 15; 00064 static const unsigned int CLOSE_GRIPPER_BUTTON = 13; 00065 static const unsigned int ARM_MODE_TOGGLE_BUTTON = 4; 00066 00067 static const unsigned int LEFT_AXIS_NUMBER = 1; 00068 static const unsigned int RIGHT_AXIS_NUMBER = 1; 00069 00070 static const unsigned int TORSO_UP_BUTTON = 12; 00071 static const unsigned int TORSO_DOWN_BUTTON = 14; 00072 00073 static const unsigned int WRIST_CLOCKWISE_BUTTON = 12; 00074 static const unsigned int WRIST_COUNTER_BUTTON = 14; 00075 00076 static const unsigned int VX_AXIS = 3; 00077 static const unsigned int VY_AXIS = 2; 00078 static const unsigned int VW_AXIS = 0; 00079 00080 static const unsigned int HEAD_PAN_AXIS = 0; 00081 static const unsigned int HEAD_TILT_AXIS = 3; 00082 00083 static const unsigned int ARM_X_AXIS = 3; 00084 static const unsigned int ARM_Y_AXIS = 2; 00085 static const unsigned int ARM_Z_AXIS = 1; 00086 static const unsigned int ARM_YAW_AXIS = 0; 00087 00088 static const unsigned int ARM_UNTUCK_BUTTON = 7; 00089 static const unsigned int ARM_TUCK_BUTTON = 5; 00090 00091 static const unsigned int MOVE_TO_WALK_ALONG_BUTTON = 0; 00092 static const unsigned int SET_WALK_ALONG_BUTTON = 3; 00093 00094 static const ros::Duration DOUBLE_TAP_TIMEOUT(.25); 00095 00096 class Pr2TeleopGeneralJoystick 00097 { 00098 public: 00099 Pr2TeleopGeneralJoystick(bool deadman_no_publish = false) 00100 { 00101 gc = NULL; 00102 } 00103 00104 void init() 00105 { 00106 des_pan_pos_ = des_tilt_pos_ = 0; 00107 vel_val_pan_ = vel_val_tilt_ = 0; 00108 00109 des_torso_pos_ = 0; 00110 00111 des_torso_vel_ = 0.0; 00112 00113 ros::NodeHandle n_local("~"); 00114 00115 // Head pan/tilt parameters 00116 n_local.param("max_pan", max_pan_, 2.7); 00117 n_local.param("max_tilt", max_tilt_, 1.4); 00118 n_local.param("min_tilt", min_tilt_, -0.4); 00119 00120 n_local.param("tilt_scale", tilt_scale_, .5); 00121 n_local.param("pan_scale", pan_scale_, 1.2); 00122 00123 n_local.param("torso_step", torso_step_, 0.05); 00124 n_local.param("min_torso", min_torso_, 0.0); 00125 n_local.param("max_torso", max_torso_, 0.3); 00126 00127 n_local.param("vx_scale", vx_scale_, 0.6); 00128 n_local.param("vy_scale", vy_scale_, 0.6); 00129 n_local.param("vw_scale", vw_scale_, 0.8); 00130 00131 n_local.param("arm_x_scale", arm_x_scale_, .15); 00132 n_local.param("arm_y_scale", arm_y_scale_, .15); 00133 n_local.param("arm_z_scale", arm_z_scale_, .15); 00134 00135 n_local.param("wrist_velocity",wrist_velocity_, 4.5); 00136 00137 n_local.param("walk_along_x_speed_scale", walk_along_x_speed_scale_, 9.0); 00138 n_local.param("walk_along_y_speed_scale", walk_along_y_speed_scale_, 9.0); 00139 n_local.param("walk_along_w_speed_scale", walk_along_w_speed_scale_, 9.0); 00140 n_local.param("walk_along_thresh", walk_along_thresh_, .015); 00141 n_local.param("walk_along_x_dist_max", walk_along_x_dist_max_, .5); 00142 n_local.param("walk_along_y_dist_max", walk_along_y_dist_max_, .5); 00143 00144 n_local.param("prosilica_namespace", prosilica_namespace_, std::string("prosilica_polled")); 00145 00146 bool control_prosilica; 00147 n_local.param("control_prosilica", control_prosilica, true); 00148 00149 bool control_body; 00150 n_local.param("control_body", control_body, true); 00151 00152 bool control_larm; 00153 n_local.param("control_larm", control_larm, true); 00154 00155 bool control_rarm; 00156 n_local.param("control_rarm", control_rarm, true); 00157 00158 bool control_head; 00159 n_local.param("control_head", control_head, true); 00160 00161 std::string arm_controller_name; 00162 n_local.param("arm_controller_name", arm_controller_name,std::string("")); 00163 00164 ROS_DEBUG("tilt scale: %.3f rad\n", tilt_scale_); 00165 ROS_DEBUG("pan scale: %.3f rad\n", pan_scale_); 00166 00167 ROS_INFO("Initing general commander"); 00168 00169 if(arm_controller_name.empty()) { 00170 gc = new GeneralCommander(control_body, 00171 control_head, 00172 control_rarm, 00173 control_larm, 00174 control_prosilica); 00175 } else { 00176 gc = new GeneralCommander(control_body, 00177 control_head, 00178 control_rarm, 00179 control_larm, 00180 control_prosilica, 00181 arm_controller_name); 00182 } 00183 first_callback_ = true; 00184 00185 head_init_ = false; 00186 torso_init_ = false; 00187 00188 proj_toggle_com_ = false; 00189 00190 walk_along_init_waiting_ = false; 00191 set_walk_along_mode_ = false; 00192 00193 joy_sub_ = n_.subscribe("joy", 10, &Pr2TeleopGeneralJoystick::joy_cb, this); 00194 } 00195 00196 00197 ~Pr2TeleopGeneralJoystick() { 00198 if(gc != NULL) { 00199 delete gc; 00200 } 00201 } 00202 00203 bool buttonOkAndOn(unsigned int buttonNum, const sensor_msgs::Joy::ConstPtr& joy_msg) const { 00204 if(buttonNum >= joy_msg->buttons.size()) return false; 00205 return(joy_msg->buttons[buttonNum]); 00206 } 00207 00208 bool axisOk(unsigned int axisNum, const sensor_msgs::Joy::ConstPtr& joy_msg) const { 00209 return (axisNum < joy_msg->axes.size()); 00210 } 00211 00212 bool sameValueAsLast(unsigned int button, 00213 const sensor_msgs::Joy::ConstPtr& new_msg, 00214 const sensor_msgs::Joy::ConstPtr& old_msg) { 00215 return (buttonOkAndOn(button, new_msg) == buttonOkAndOn(button, old_msg)); 00216 } 00217 00218 00219 void joy_cb(const sensor_msgs::Joy::ConstPtr& joy_msg) 00220 { 00221 if(first_callback_) { 00222 last_joy_ = joy_msg; 00223 first_callback_ = false; 00224 } 00225 00226 JoystickLayoutMode layout; 00227 00228 if(buttonOkAndOn(BODY_LAYOUT_BUTTON, joy_msg)) { 00229 layout = LAYOUT_BODY; 00230 } else if (buttonOkAndOn(RIGHT_ARM_LAYOUT_BUTTON, joy_msg) && buttonOkAndOn(LEFT_ARM_LAYOUT_BUTTON, joy_msg)) { 00231 layout = LAYOUT_BOTH_ARMS; 00232 } else if (buttonOkAndOn(RIGHT_ARM_LAYOUT_BUTTON,joy_msg)) { 00233 layout = LAYOUT_RIGHT_ARM; 00234 } else if (buttonOkAndOn(LEFT_ARM_LAYOUT_BUTTON, joy_msg)) { 00235 layout = LAYOUT_LEFT_ARM; 00236 } else if (buttonOkAndOn(HEAD_LAYOUT_BUTTON, joy_msg)) { 00237 layout = LAYOUT_HEAD; 00238 } else { 00239 layout = LAYOUT_NONE; 00240 } 00241 00242 bool setting_walk_along_this_cycle_ = false; 00243 00244 if(layout == LAYOUT_HEAD) { 00245 if(buttonOkAndOn(MOVE_TO_WALK_ALONG_BUTTON, joy_msg) && !sameValueAsLast(MOVE_TO_WALK_ALONG_BUTTON, joy_msg, last_joy_)) { 00246 ros::Time now = ros::Time::now(); 00247 if(now-last_walk_along_time_ < DOUBLE_TAP_TIMEOUT) { 00248 //only matters if this is off 00249 if(!gc->isWalkAlongOk()) { 00250 set_walk_along_mode_ = true; 00251 setting_walk_along_this_cycle_ = true; 00252 } 00253 } 00254 if(gc->isWalkAlongOk()) { 00255 gc->turnOffWalkAlong(); 00256 ROS_INFO("Turning off walk along"); 00257 } else { 00258 last_walk_along_time_ = now; 00259 } 00260 } 00261 } 00262 00263 bool in_walk_along = false; 00264 if(gc->isWalkAlongOk()) { 00265 if(buttonOkAndOn(MOVE_TO_WALK_ALONG_BUTTON, joy_msg) && !sameValueAsLast(MOVE_TO_WALK_ALONG_BUTTON, joy_msg, last_joy_)) { 00266 gc->turnOffWalkAlong(); 00267 ROS_INFO("Turning off walk along"); 00268 } else { 00269 vel_val_pan_ = 0.0; 00270 vel_val_tilt_ = 0.0; 00271 des_torso_vel_ = 0.0; 00272 des_vx_ = 0.0; 00273 des_vy_ = 0.0; 00274 des_vw_ = 0.0; 00275 des_right_wrist_vel_ = 0.0; 00276 right_arm_vx_ = 0.0; 00277 right_arm_vy_ = 0.0; 00278 right_arm_vz_ = 0.0; 00279 des_left_wrist_vel_ = 0.0; 00280 left_arm_vx_ = 0.0; 00281 left_arm_vy_ = 0.0; 00282 left_arm_vz_ = 0.0; 00283 in_walk_along = true; 00284 } 00285 } 00286 00287 //we must be moving the arms into the mode 00288 if(!in_walk_along && layout == LAYOUT_HEAD && set_walk_along_mode_) { 00289 if(buttonOkAndOn(SET_WALK_ALONG_BUTTON, joy_msg) 00290 && !sameValueAsLast(SET_WALK_ALONG_BUTTON, joy_msg, last_joy_)) { 00291 gc->sendHeadSequence(GeneralCommander::HEAD_SHAKE); 00292 } 00293 if(!setting_walk_along_this_cycle_ && buttonOkAndOn(MOVE_TO_WALK_ALONG_BUTTON, joy_msg) && !sameValueAsLast(MOVE_TO_WALK_ALONG_BUTTON, joy_msg, last_joy_)) { 00294 set_walk_along_mode_ = false; 00295 ROS_INFO("No longer waiting for set"); 00296 } 00297 } 00298 00299 if(!in_walk_along && layout == LAYOUT_HEAD && walk_along_init_waiting_) { 00300 if(buttonOkAndOn(SET_WALK_ALONG_BUTTON, joy_msg) 00301 && !sameValueAsLast(SET_WALK_ALONG_BUTTON, joy_msg, last_joy_)) { 00302 bool ok = gc->initWalkAlong(); 00303 if(!ok) { 00304 gc->sendHeadSequence(GeneralCommander::HEAD_SHAKE); 00305 } else { 00306 gc->sendHeadSequence(GeneralCommander::HEAD_NOD); 00307 ROS_INFO("Should be in walk along"); 00308 walk_along_init_waiting_ = false; 00309 } 00310 } 00311 if(buttonOkAndOn(MOVE_TO_WALK_ALONG_BUTTON, joy_msg) && !sameValueAsLast(MOVE_TO_WALK_ALONG_BUTTON, joy_msg, last_joy_)) { 00312 walk_along_init_waiting_ = false; 00313 ROS_INFO("No longer waiting for init"); 00314 } 00315 } 00316 if(layout == LAYOUT_RIGHT_ARM || layout == LAYOUT_LEFT_ARM || layout == LAYOUT_BOTH_ARMS) { 00317 if(buttonOkAndOn(CLOSE_GRIPPER_BUTTON, joy_msg) 00318 && !sameValueAsLast(CLOSE_GRIPPER_BUTTON, joy_msg, last_joy_)) { 00319 if(layout == LAYOUT_RIGHT_ARM) { 00320 gc->sendGripperCommand(GeneralCommander::ARMS_RIGHT, false); 00321 } else if(layout == LAYOUT_LEFT_ARM) { 00322 gc->sendGripperCommand(GeneralCommander::ARMS_LEFT, false); 00323 } else { 00324 gc->sendGripperCommand(GeneralCommander::ARMS_BOTH, false); 00325 } 00326 } 00327 if(buttonOkAndOn(OPEN_GRIPPER_BUTTON, joy_msg) 00328 && !sameValueAsLast(OPEN_GRIPPER_BUTTON, joy_msg, last_joy_)) { 00329 if(layout == LAYOUT_RIGHT_ARM) { 00330 gc->sendGripperCommand(GeneralCommander::ARMS_RIGHT, true); 00331 } else if(layout == LAYOUT_LEFT_ARM) { 00332 gc->sendGripperCommand(GeneralCommander::ARMS_LEFT, true); 00333 } else { 00334 gc->sendGripperCommand(GeneralCommander::ARMS_BOTH, true); 00335 } 00336 } 00337 } 00338 00339 if(!in_walk_along && layout == LAYOUT_HEAD) { 00340 00341 if(buttonOkAndOn(PROJECTOR_TOGGLE_BUTTON, joy_msg) 00342 && !sameValueAsLast(PROJECTOR_TOGGLE_BUTTON,joy_msg, last_joy_)) { 00343 proj_toggle_com_ = !proj_toggle_com_; 00344 gc->sendProjectorStartStop(proj_toggle_com_); 00345 } 00346 00347 if(buttonOkAndOn(PROSILICA_POLL_BUTTON, joy_msg) 00348 && !sameValueAsLast(PROSILICA_POLL_BUTTON,joy_msg, last_joy_)) { 00349 gc->requestProsilicaImage(prosilica_namespace_); 00350 } 00351 00352 if(buttonOkAndOn(HEAD_MODE_TOGGLE_BUTTON, joy_msg) 00353 && !sameValueAsLast(HEAD_MODE_TOGGLE_BUTTON, joy_msg, last_joy_)) { 00354 00355 if(gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) { 00356 ROS_DEBUG("Head mode to left"); 00357 gc->setHeadMode(GeneralCommander::HEAD_TRACK_LEFT_HAND); 00358 } else if(gc->getHeadMode() == GeneralCommander::HEAD_TRACK_LEFT_HAND) { 00359 gc->setHeadMode(GeneralCommander::HEAD_TRACK_RIGHT_HAND); 00360 ROS_DEBUG("Head mode to right"); 00361 } else if(gc->getHeadMode() == GeneralCommander::HEAD_TRACK_RIGHT_HAND){ 00362 gc->setHeadMode(GeneralCommander::HEAD_MANNEQUIN); 00363 ROS_DEBUG("Head mode to mannequin"); 00364 } else { 00365 ROS_DEBUG("Head mode to joystick"); 00366 head_init_ = false; 00367 gc->setHeadMode(GeneralCommander::HEAD_JOYSTICK); 00368 } 00369 } 00370 00371 if(buttonOkAndOn(LASER_TOGGLE_BUTTON, joy_msg) 00372 && !sameValueAsLast(LASER_TOGGLE_BUTTON, joy_msg, last_joy_)) { 00373 if(gc->getLaserMode() == GeneralCommander::LASER_TILT_OFF) { 00374 gc->setLaserMode(GeneralCommander::LASER_TILT_SLOW); 00375 } else if(gc->getLaserMode() == GeneralCommander::LASER_TILT_SLOW) { 00376 gc->setLaserMode(GeneralCommander::LASER_TILT_FAST); 00377 } else { 00378 gc->setLaserMode(GeneralCommander::LASER_TILT_OFF); 00379 } 00380 } 00381 00382 if(axisOk(HEAD_PAN_AXIS, joy_msg)) 00383 { 00384 vel_val_pan_ = joy_msg->axes[HEAD_PAN_AXIS] * pan_scale_; 00385 } 00386 00387 if(axisOk(HEAD_TILT_AXIS, joy_msg)) 00388 { 00389 vel_val_tilt_ = joy_msg->axes[HEAD_TILT_AXIS] * tilt_scale_; 00390 } 00391 } else { 00392 vel_val_pan_ = 0.0; 00393 vel_val_tilt_ = 0.0; 00394 } 00395 00396 if(!in_walk_along && layout == LAYOUT_BODY) { 00397 bool down = buttonOkAndOn(TORSO_DOWN_BUTTON, joy_msg); 00398 bool up = buttonOkAndOn(TORSO_UP_BUTTON, joy_msg); 00399 00400 //ROS_INFO_STREAM("Down is " << down); 00401 //ROS_INFO_STREAM("Up is " << up); 00402 00403 if(down && !up) { 00404 des_torso_vel_ = -torso_step_; 00405 } else if(!down && up) { 00406 des_torso_vel_ = torso_step_; 00407 } else { 00408 //ROS_INFO_STREAM("Setting des vel to 0.0"); 00409 des_torso_vel_ = 0.0; 00410 } 00411 if(axisOk(VX_AXIS, joy_msg)) { 00412 des_vx_ = joy_msg->axes[VX_AXIS]*vx_scale_; 00413 } else { 00414 des_vx_ = 0.0; 00415 } 00416 if(axisOk(VY_AXIS, joy_msg)) { 00417 des_vy_ = joy_msg->axes[VY_AXIS]*vy_scale_; 00418 } else { 00419 des_vy_ = 0.0; 00420 } 00421 if(axisOk(VW_AXIS, joy_msg)) { 00422 des_vw_ = joy_msg->axes[VW_AXIS]*vw_scale_; 00423 } else { 00424 des_vw_ = 0.0; 00425 } 00426 } else { 00427 des_torso_vel_ = 0.0; 00428 des_vx_ = 0.0; 00429 des_vy_ = 0.0; 00430 des_vw_ = 0.0; 00431 } 00432 00433 if(layout == LAYOUT_RIGHT_ARM) { 00434 if(buttonOkAndOn(ARM_MODE_TOGGLE_BUTTON, joy_msg) && !sameValueAsLast(ARM_MODE_TOGGLE_BUTTON, joy_msg, last_joy_)) { 00435 if(in_walk_along) { 00436 gc->turnOffWalkAlong(); 00437 ROS_INFO("Turning off walk along"); 00438 } 00439 if(gc->getArmMode(GeneralCommander::ARMS_RIGHT) == GeneralCommander::ARM_POSITION_CONTROL) { 00440 gc->setArmMode(GeneralCommander::ARMS_RIGHT,GeneralCommander::ARM_MANNEQUIN_MODE); 00441 } else if (gc->getArmMode(GeneralCommander::ARMS_RIGHT) == GeneralCommander::ARM_MANNEQUIN_MODE) { 00442 gc->setArmMode(GeneralCommander::ARMS_RIGHT,GeneralCommander::ARM_NO_CONTROLLER); 00443 } else { 00444 gc->setArmMode(GeneralCommander::ARMS_RIGHT,GeneralCommander::ARM_POSITION_CONTROL); 00445 } 00446 } 00447 00448 if(buttonOkAndOn(ARM_TUCK_BUTTON, joy_msg) && !sameValueAsLast(ARM_TUCK_BUTTON, joy_msg, last_joy_)) { 00449 if(in_walk_along) { 00450 gc->turnOffWalkAlong(); 00451 ROS_INFO("Turning off walk along"); 00452 } 00453 gc->tuckArms(GeneralCommander::ARMS_RIGHT); 00454 } else if(buttonOkAndOn(ARM_UNTUCK_BUTTON, joy_msg) && !sameValueAsLast(ARM_UNTUCK_BUTTON, joy_msg, last_joy_)) { 00455 if(in_walk_along) { 00456 gc->turnOffWalkAlong(); 00457 ROS_INFO("Turning off walk along"); 00458 } 00459 gc->untuckArms(GeneralCommander::ARMS_RIGHT); 00460 } 00461 00462 if(!in_walk_along) { 00463 00464 bool lookAnalog = false; 00465 bool rotClock = buttonOkAndOn(WRIST_CLOCKWISE_BUTTON, joy_msg); 00466 bool rotCounter = buttonOkAndOn(WRIST_COUNTER_BUTTON, joy_msg); 00467 if(rotClock && !rotCounter) { 00468 des_right_wrist_vel_ = wrist_velocity_; 00469 } else if(!rotClock && rotCounter) { 00470 des_right_wrist_vel_ = -wrist_velocity_; 00471 } else { 00472 des_right_wrist_vel_ = 0.0; 00473 lookAnalog = true; 00474 } 00475 00476 if(lookAnalog) { 00477 //look at analog sticks if we aren't supposed to wrist rotate 00478 if(axisOk(ARM_X_AXIS, joy_msg)) { 00479 right_arm_vx_ = joy_msg->axes[ARM_X_AXIS]*arm_x_scale_; 00480 } else { 00481 right_arm_vx_ = 0.0; 00482 } 00483 if(axisOk(ARM_Y_AXIS, joy_msg)) { 00484 right_arm_vy_ = joy_msg->axes[ARM_Y_AXIS]*arm_y_scale_; 00485 } else { 00486 right_arm_vy_ = 0.0; 00487 } 00488 if(axisOk(ARM_Z_AXIS, joy_msg)) { 00489 right_arm_vz_ = joy_msg->axes[ARM_Z_AXIS]*arm_z_scale_; 00490 } else { 00491 right_arm_vz_ = 0.0; 00492 } 00493 //ROS_INFO_STREAM("Setting vx " << right_arm_vx_ << " " << right_arm_vy_ << " " << right_arm_vz_); 00494 } else { 00495 right_arm_vx_ = 0.0; 00496 right_arm_vy_ = 0.0; 00497 right_arm_vz_ = 0.0; 00498 } 00499 } 00500 } else if (layout != LAYOUT_BOTH_ARMS) { 00501 des_right_wrist_vel_ = 0.0; 00502 right_arm_vx_ = 0.0; 00503 right_arm_vy_ = 0.0; 00504 right_arm_vz_ = 0.0; 00505 } 00506 if(layout == LAYOUT_LEFT_ARM) { 00507 if(buttonOkAndOn(ARM_MODE_TOGGLE_BUTTON, joy_msg) && !sameValueAsLast(ARM_MODE_TOGGLE_BUTTON, joy_msg, last_joy_)) { 00508 if(in_walk_along) { 00509 gc->turnOffWalkAlong(); 00510 ROS_INFO("Turning off walk along"); 00511 } 00512 if(gc->getArmMode(GeneralCommander::ARMS_LEFT) == GeneralCommander::ARM_POSITION_CONTROL) { 00513 gc->setArmMode(GeneralCommander::ARMS_LEFT,GeneralCommander::ARM_MANNEQUIN_MODE); 00514 } else if (gc->getArmMode(GeneralCommander::ARMS_LEFT) == GeneralCommander::ARM_MANNEQUIN_MODE) { 00515 gc->setArmMode(GeneralCommander::ARMS_LEFT,GeneralCommander::ARM_NO_CONTROLLER); 00516 } else { 00517 gc->setArmMode(GeneralCommander::ARMS_LEFT,GeneralCommander::ARM_POSITION_CONTROL); 00518 } 00519 } 00520 00521 if(buttonOkAndOn(ARM_TUCK_BUTTON, joy_msg) && !sameValueAsLast(ARM_TUCK_BUTTON, joy_msg, last_joy_)) { 00522 if(in_walk_along) { 00523 gc->turnOffWalkAlong(); 00524 ROS_INFO("Turning off walk along"); 00525 } 00526 gc->tuckArms(GeneralCommander::ARMS_LEFT); 00527 } else if(buttonOkAndOn(ARM_UNTUCK_BUTTON, joy_msg) && !sameValueAsLast(ARM_UNTUCK_BUTTON, joy_msg, last_joy_)) { 00528 if(in_walk_along) { 00529 gc->turnOffWalkAlong(); 00530 ROS_INFO("Turning off walk along"); 00531 } 00532 gc->untuckArms(GeneralCommander::ARMS_LEFT); 00533 } 00534 00535 if(!in_walk_along) { 00536 bool lookAnalog = false; 00537 bool rotClock = buttonOkAndOn(WRIST_CLOCKWISE_BUTTON, joy_msg); 00538 bool rotCounter = buttonOkAndOn(WRIST_COUNTER_BUTTON, joy_msg); 00539 if(rotClock && !rotCounter) { 00540 des_left_wrist_vel_ = wrist_velocity_; 00541 } else if(!rotClock && rotCounter) { 00542 des_left_wrist_vel_ = -wrist_velocity_; 00543 } else { 00544 des_left_wrist_vel_ = 0.0; 00545 lookAnalog = true; 00546 } 00547 00548 if(lookAnalog) { 00549 //look at analog sticks if we aren't supposed to wrist rotate 00550 if(axisOk(ARM_X_AXIS, joy_msg)) { 00551 left_arm_vx_ = joy_msg->axes[ARM_X_AXIS]*arm_x_scale_; 00552 } else { 00553 left_arm_vx_ = 0.0; 00554 } 00555 if(axisOk(ARM_Y_AXIS, joy_msg)) { 00556 left_arm_vy_ = joy_msg->axes[ARM_Y_AXIS]*arm_y_scale_; 00557 } else { 00558 left_arm_vy_ = 0.0; 00559 } 00560 if(axisOk(ARM_Z_AXIS, joy_msg)) { 00561 left_arm_vz_ = joy_msg->axes[ARM_Z_AXIS]*arm_z_scale_; 00562 } else { 00563 left_arm_vz_ = 0.0; 00564 } 00565 //ROS_INFO_STREAM("Setting vx " << left_arm_vx_ << " " << left_arm_vy_ << " " << left_arm_vz_); 00566 } else { 00567 left_arm_vx_ = 0.0; 00568 left_arm_vy_ = 0.0; 00569 left_arm_vz_ = 0.0; 00570 } 00571 } 00572 } else if (layout != LAYOUT_BOTH_ARMS) { 00573 des_left_wrist_vel_ = 0.0; 00574 left_arm_vx_ = 0.0; 00575 left_arm_vy_ = 0.0; 00576 left_arm_vz_ = 0.0; 00577 } 00578 if(layout == LAYOUT_BOTH_ARMS) { 00579 if(buttonOkAndOn(ARM_MODE_TOGGLE_BUTTON, joy_msg) && !sameValueAsLast(ARM_MODE_TOGGLE_BUTTON, joy_msg, last_joy_)) { 00580 GeneralCommander::ArmControlMode toSend; 00581 if(in_walk_along) { 00582 gc->turnOffWalkAlong(); 00583 ROS_INFO("Turning off walk along"); 00584 } 00585 if(gc->getArmMode(GeneralCommander::ARMS_RIGHT) != gc->getArmMode(GeneralCommander::ARMS_RIGHT)) { 00586 toSend = GeneralCommander::ARM_POSITION_CONTROL; 00587 } else if(gc->getArmMode(GeneralCommander::ARMS_RIGHT) == GeneralCommander::ARM_POSITION_CONTROL) { 00588 toSend = GeneralCommander::ARM_MANNEQUIN_MODE; 00589 } else if (gc->getArmMode(GeneralCommander::ARMS_RIGHT) == GeneralCommander::ARM_MANNEQUIN_MODE) { 00590 toSend = GeneralCommander::ARM_NO_CONTROLLER; 00591 } else { 00592 toSend = GeneralCommander::ARM_POSITION_CONTROL; 00593 } 00594 gc->setArmMode(GeneralCommander::ARMS_BOTH, toSend); 00595 } 00596 00597 if(buttonOkAndOn(ARM_TUCK_BUTTON, joy_msg) && !sameValueAsLast(ARM_TUCK_BUTTON, joy_msg, last_joy_)) { 00598 if(in_walk_along) { 00599 gc->turnOffWalkAlong(); 00600 ROS_INFO("Turning off walk along"); 00601 } 00602 gc->tuckArms(GeneralCommander::ARMS_BOTH); 00603 } else if(buttonOkAndOn(ARM_UNTUCK_BUTTON, joy_msg) && !sameValueAsLast(ARM_UNTUCK_BUTTON, joy_msg, last_joy_)) { 00604 if(in_walk_along) { 00605 gc->turnOffWalkAlong(); 00606 ROS_INFO("Turning off walk along"); 00607 } 00608 gc->untuckArms(GeneralCommander::ARMS_BOTH); 00609 } 00610 00611 if(!in_walk_along) { 00612 bool lookAnalog = false; 00613 bool rotClock = buttonOkAndOn(WRIST_CLOCKWISE_BUTTON, joy_msg); 00614 bool rotCounter = buttonOkAndOn(WRIST_COUNTER_BUTTON, joy_msg); 00615 if(rotClock && !rotCounter) { 00616 des_left_wrist_vel_ = wrist_velocity_; 00617 des_right_wrist_vel_ = wrist_velocity_; 00618 } else if(!rotClock && rotCounter) { 00619 des_left_wrist_vel_ = -wrist_velocity_; 00620 des_right_wrist_vel_ = -wrist_velocity_; 00621 } else { 00622 des_left_wrist_vel_ = 0.0; 00623 des_right_wrist_vel_ = 0.0; 00624 lookAnalog = true; 00625 } 00626 00627 if(lookAnalog) { 00628 //look at analog sticks if we aren't supposed to wrist rotate 00629 if(axisOk(ARM_X_AXIS, joy_msg)) { 00630 left_arm_vx_ = joy_msg->axes[ARM_X_AXIS]*arm_x_scale_; 00631 right_arm_vx_ = joy_msg->axes[ARM_X_AXIS]*arm_x_scale_; 00632 } else { 00633 left_arm_vx_ = 0.0; 00634 right_arm_vz_ = 0.0; 00635 } 00636 if(axisOk(ARM_Y_AXIS, joy_msg)) { 00637 left_arm_vy_ = joy_msg->axes[ARM_Y_AXIS]*arm_y_scale_; 00638 right_arm_vy_ = joy_msg->axes[ARM_Y_AXIS]*arm_y_scale_; 00639 } else { 00640 left_arm_vy_ = 0.0; 00641 right_arm_vz_ = 0.0; 00642 } 00643 if(axisOk(ARM_Z_AXIS, joy_msg)) { 00644 left_arm_vz_ = joy_msg->axes[ARM_Z_AXIS]*arm_z_scale_; 00645 right_arm_vz_ = joy_msg->axes[ARM_Z_AXIS]*arm_z_scale_; 00646 } else { 00647 left_arm_vz_ = 0.0; 00648 right_arm_vz_ = 0.0; 00649 } 00650 //ROS_INFO_STREAM("Setting vx " << left_arm_vx_ << " " << left_arm_vy_ << " " << left_arm_vz_); 00651 } else { 00652 left_arm_vx_ = 0.0; 00653 left_arm_vy_ = 0.0; 00654 left_arm_vz_ = 0.0; 00655 right_arm_vx_ = 0.0; 00656 right_arm_vy_ = 0.0; 00657 right_arm_vz_ = 0.0; 00658 } 00659 } 00660 } else if (layout != LAYOUT_RIGHT_ARM && layout != LAYOUT_LEFT_ARM) { 00661 des_right_wrist_vel_ = 0.0; 00662 des_left_wrist_vel_ = 0.0; 00663 left_arm_vx_ = 0.0; 00664 left_arm_vy_ = 0.0; 00665 left_arm_vz_ = 0.0; 00666 right_arm_vx_ = 0.0; 00667 right_arm_vy_ = 0.0; 00668 right_arm_vz_ = 0.0; 00669 } 00670 00671 joy_deadman_ = ros::Time::now(); 00672 last_joy_ = joy_msg; 00673 } 00674 00675 bool convertCurrentVelToDesiredTorsoPos(double hz) { 00676 if(!torso_init_) { 00677 double cur_torso_pos = 0.0; 00678 bool ok = gc->getJointPosition("torso_lift_joint", cur_torso_pos); 00679 if(!ok) return false; 00680 des_torso_pos_ = cur_torso_pos; 00681 torso_init_ = true; 00682 } 00683 double dt = 1.0/double(hz); 00684 double horizon = dt*5.0; 00685 double cur_torso_pos = 0.0; 00686 gc->getJointPosition("torso_lift_joint", cur_torso_pos); 00687 des_torso_pos_ = cur_torso_pos+des_torso_vel_ * horizon; 00688 des_torso_pos_ = std::min(des_torso_pos_, max_torso_); 00689 des_torso_pos_ = std::max(des_torso_pos_, min_torso_); 00690 return true; 00691 } 00692 00693 bool convertCurrentVelToDesiredHeadPos(double hz) { 00694 00695 if(!head_init_) { 00696 double cur_pan_pos = 0.0; 00697 double cur_tilt_pos = 0.0; 00698 bool ok = gc->getJointPosition("head_pan_joint", cur_pan_pos); 00699 if(!ok) return false; 00700 ok = gc->getJointPosition("head_tilt_joint", cur_tilt_pos); 00701 if(!ok) return false; 00702 des_pan_pos_ = cur_pan_pos; 00703 des_tilt_pos_ = cur_tilt_pos; 00704 head_init_ = true; 00705 } 00706 if(fabs(vel_val_pan_) > .0001) { 00707 des_pan_pos_ = des_pan_pos_ + vel_val_pan_*(1.0/hz); 00708 des_pan_pos_ = std::min(des_pan_pos_, max_pan_); 00709 des_pan_pos_ = std::max(des_pan_pos_, -max_pan_); 00710 } 00711 if(fabs(vel_val_tilt_) > .0001) { 00712 des_tilt_pos_ = des_tilt_pos_ - vel_val_tilt_*(1.0/hz); 00713 des_tilt_pos_ = std::min(des_tilt_pos_, max_tilt_); 00714 des_tilt_pos_ = std::max(des_tilt_pos_, min_tilt_); 00715 } 00716 //ROS_INFO_STREAM("Des pan pos " << des_pan_pos_ << " tilt " << des_tilt_pos_); 00717 return true; 00718 } 00719 00720 public: 00721 double max_pan_, max_tilt_, min_tilt_; 00722 int axis_pan_, axis_tilt_; 00723 double pan_scale_, tilt_scale_; 00724 00725 double des_pan_pos_; 00726 double des_tilt_pos_; 00727 00728 double vel_val_pan_; 00729 double vel_val_tilt_; 00730 00731 double des_vx_; 00732 double des_vy_; 00733 double des_vw_; 00734 00735 double vx_scale_; 00736 double vy_scale_; 00737 double vw_scale_; 00738 00739 double arm_x_scale_; 00740 double arm_y_scale_; 00741 double arm_z_scale_; 00742 00743 double right_arm_vx_; 00744 double right_arm_vy_; 00745 double right_arm_vz_; 00746 00747 double left_arm_vx_; 00748 double left_arm_vy_; 00749 double left_arm_vz_; 00750 00751 bool head_init_; 00752 bool torso_init_; 00753 00754 double req_torso_vel_; 00755 double req_torso_pos_; 00756 00757 double des_torso_pos_; 00758 double des_torso_vel_; 00759 double torso_step_; 00760 double min_torso_; 00761 double max_torso_; 00762 00763 double wrist_velocity_; 00764 double des_right_wrist_vel_; 00765 double des_left_wrist_vel_; 00766 00767 double walk_along_x_speed_scale_; 00768 double walk_along_y_speed_scale_; 00769 double walk_along_w_speed_scale_; 00770 double walk_along_thresh_; 00771 double walk_along_x_dist_max_; 00772 double walk_along_y_dist_max_; 00773 00774 bool walk_along_init_waiting_; 00775 bool set_walk_along_mode_; 00776 00777 std::string prosilica_namespace_; 00778 00779 bool proj_toggle_com_; 00780 00781 int projector_toggle_button_; 00782 int tilt_toggle_button_; 00783 int switch_head_control_mode_button_; 00784 00785 GeneralCommander* gc; 00786 00787 ros::Time joy_deadman_; 00788 00789 sensor_msgs::JoyConstPtr last_joy_; 00790 bool first_callback_; 00791 00792 ros::NodeHandle n_; 00793 ros::Subscriber joy_sub_; 00794 00795 ros::Time last_projector_toggle_; 00796 ros::Time last_laser_toggle_; 00797 ros::Time last_head_toggle_; 00798 00799 ros::Time last_walk_along_time_; 00800 00801 }; 00802 00803 static const double FastHz = 100; 00804 static const double SlowHz = 20; 00805 00806 void spin_function() { 00807 ros::spin(); 00808 } 00809 00810 // void quit(int sig) 00811 // { 00812 // delete generaljoy; 00813 // ros::shutdown(); 00814 // spin_thread.join(); 00815 // exit(0); 00816 // } 00817 00818 int main(int argc, char **argv) 00819 { 00820 ros::init(argc, argv, "pr2_telep_general_joystick", ros::init_options::NoSigintHandler); 00821 //signal(SIGINT,quit); 00822 00823 boost::thread spin_thread(boost::bind(&spin_function)); 00824 00825 Pr2TeleopGeneralJoystick generaljoy; 00826 generaljoy.init(); 00827 00828 ros::Rate pub_rate(FastHz); 00829 00830 unsigned int counter_limit = (unsigned int)(FastHz/SlowHz); 00831 00832 unsigned int counter = 0; 00833 00834 ros::Time beforeCall = ros::Time::now(); 00835 ros::Time afterCall = ros::Time::now(); 00836 while (ros::ok()) 00837 { 00838 //ROS_INFO_STREAM("Time since last " << (ros::Time::now()-beforeCall).toSec()); 00839 beforeCall = ros::Time::now(); 00840 00841 if(!generaljoy.gc->isWalkAlongOk() && !generaljoy.set_walk_along_mode_ && !generaljoy.walk_along_init_waiting_) { 00842 if(generaljoy.convertCurrentVelToDesiredHeadPos(FastHz)) { 00843 generaljoy.gc->sendHeadCommand(generaljoy.des_pan_pos_, generaljoy.des_tilt_pos_); 00844 } 00845 generaljoy.gc->sendHeadTrackCommand(); 00846 generaljoy.gc->sendBaseCommand(generaljoy.des_vx_, generaljoy.des_vy_, generaljoy.des_vw_); 00847 } 00848 00849 if((counter % counter_limit) == 0) { 00850 00851 counter = 0; 00852 00853 if(generaljoy.set_walk_along_mode_) { 00854 bool ok = generaljoy.gc->moveToWalkAlongArmPose(); 00855 //if we didn't get a select while moving the arms 00856 if(ok && generaljoy.set_walk_along_mode_) { 00857 ROS_INFO("Arms in walk position"); 00858 generaljoy.walk_along_init_waiting_ = true; 00859 } else { 00860 ROS_INFO("Arms not in walk position"); 00861 } 00862 generaljoy.set_walk_along_mode_ = false; 00863 } 00864 00865 if(generaljoy.gc->isWalkAlongOk()) { 00866 00867 generaljoy.gc->sendWalkAlongCommand(generaljoy.walk_along_thresh_, 00868 generaljoy.walk_along_x_dist_max_, 00869 generaljoy.walk_along_x_speed_scale_, 00870 generaljoy.walk_along_y_dist_max_, 00871 generaljoy.walk_along_y_speed_scale_, 00872 generaljoy.walk_along_w_speed_scale_); 00873 } else { 00874 if(generaljoy.convertCurrentVelToDesiredTorsoPos(SlowHz)) { 00875 generaljoy.gc->sendTorsoCommand(generaljoy.des_torso_pos_, generaljoy.des_torso_vel_); 00876 } 00877 //generaljoy.gc->updateCurrentWristPositions(); 00878 generaljoy.gc->sendWristVelCommands(generaljoy.des_right_wrist_vel_, generaljoy.des_left_wrist_vel_, SlowHz); 00879 00880 generaljoy.gc->sendArmVelCommands(generaljoy.right_arm_vx_, generaljoy.right_arm_vy_, generaljoy.right_arm_vz_, 0.0, 0.0, 00881 generaljoy.left_arm_vx_, generaljoy.left_arm_vy_, generaljoy.left_arm_vz_, 0.0, 0.0, 00882 SlowHz); 00883 } 00884 } 00885 //ROS_INFO_STREAM("Everything took " << (afterCall-beforeCall).toSec()); 00886 counter++; 00887 pub_rate.sleep(); 00888 } 00889 00890 ros::shutdown(); 00891 spin_thread.join(); 00892 return 0; 00893 }