$search
00001 /* 00002 * pr2_teleop_booth_commander 00003 * Copyright (c) 2008, 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 <string> 00034 #include <boost/bind.hpp> 00035 00036 #include <pr2_mechanism_msgs/SwitchController.h> 00037 #include <geometry_msgs/Twist.h> 00038 #include <trajectory_msgs/JointTrajectory.h> 00039 #include <kinematics_msgs/GetKinematicSolverInfo.h> 00040 #include <kinematics_msgs/GetPositionFK.h> 00041 #include <kinematics_msgs/GetPositionIK.h> 00042 #include <polled_camera/GetPolledImage.h> 00043 00044 #include "pr2_teleop_general/pr2_teleop_general_commander.h" 00045 00046 #include "urdf_interface/pose.h" 00047 00048 static const std::string LEFT_HAND_LINK_TO_TRACK = "l_gripper_palm_link"; 00049 static const std::string RIGHT_HAND_LINK_TO_TRACK = "r_gripper_palm_link"; 00050 00051 static const double MAX_HEAD_TRACK_SPEED = 2.0; 00052 00053 static const double GRIPPER_CLOSE_POSITION = 0.000; 00054 static const double GRIPPER_CLOSE_MAX_EFFORT = 10000.0; 00055 00056 static const double GRIPPER_OPEN_POSITION = 0.086; 00057 static const double GRIPPER_OPEN_MAX_EFFORT = 10000.0; 00058 00059 static const std::string RIGHT_ARM_MANNEQUIN_CONTROLLER = "r_arm_controller_loose"; 00060 static const std::string LEFT_ARM_MANNEQUIN_CONTROLLER = "l_arm_controller_loose"; 00061 00062 static const std::string HEAD_MANNEQUIN_CONTROLLER = "head_traj_controller_loose"; 00063 static const std::string HEAD_POSITION_CONTROLLER = "head_traj_controller"; 00064 00065 static const unsigned int WALK_BUFFER = 10; 00066 00067 GeneralCommander::GeneralCommander(bool control_body, 00068 bool control_head, 00069 bool control_rarm, 00070 bool control_larm, 00071 bool control_prosilica, 00072 std::string arm_controller_name) 00073 : n_(), 00074 control_body_(control_body), 00075 control_head_(control_head), 00076 control_rarm_(control_rarm), 00077 control_larm_(control_larm), 00078 control_prosilica_(control_prosilica) 00079 { 00080 00081 r_arm_controller_name_ = "r_"+arm_controller_name; 00082 l_arm_controller_name_ = "l_"+arm_controller_name; 00083 00084 head_control_mode_ = HEAD_JOYSTICK; 00085 00086 std::string urdf_xml,full_urdf_xml; 00087 n_.param("urdf_xml", urdf_xml, std::string("robot_description")); 00088 if(!n_.getParam(urdf_xml,full_urdf_xml)) 00089 { 00090 ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str()); 00091 robot_model_initialized_ = false; 00092 } 00093 else 00094 { 00095 robot_model_.initString(full_urdf_xml); 00096 robot_model_initialized_ = true; 00097 } 00098 00099 //universal 00100 switch_controllers_service_ = n_.serviceClient<pr2_mechanism_msgs::SwitchController>("pr2_controller_manager/switch_controller"); 00101 joint_state_sub_ = n_.subscribe("joint_states", 1, &GeneralCommander::jointStateCallback, this); 00102 power_board_sub_ = n_.subscribe<pr2_msgs::PowerBoardState>("power_board/state", 1, &GeneralCommander::powerBoardCallback, this); 00103 00104 if(control_head_) { 00105 tilt_laser_service_ = n_.serviceClient<pr2_msgs::SetPeriodicCmd>("laser_tilt_controller/set_periodic_cmd"); 00106 head_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>("head_traj_controller/command", 1); 00107 head_track_hand_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction>("/head_traj_controller/point_head_action", true); 00108 while(!head_track_hand_client_->waitForServer(ros::Duration(5.0))){ 00109 ROS_INFO("Waiting for the point_head_action server to come up"); 00110 } 00111 //just in case there's an existing goal 00112 head_track_hand_client_->cancelAllGoals(); 00113 } else { 00114 head_track_hand_client_ = NULL; 00115 } 00116 if(control_body_) { 00117 torso_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>("torso_controller/command", 1); 00118 base_pub_ = n_.advertise<geometry_msgs::Twist>("base_controller/command", 1); 00119 } 00120 if(control_rarm_) { 00121 right_gripper_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>("r_gripper_controller/gripper_action", true); 00122 right_arm_trajectory_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>(r_arm_controller_name_+"/joint_trajectory_action", true); 00123 right_arm_traj_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(r_arm_controller_name_+"/command", 1); 00124 while(!right_gripper_client_->waitForServer(ros::Duration(5.0))){ 00125 ROS_INFO("Waiting for the right gripper action server to come up"); 00126 } 00127 while(!right_arm_trajectory_client_->waitForServer(ros::Duration(5.0))){ 00128 ROS_INFO_STREAM("Waiting for the right arm trajectory action server to come up" << r_arm_controller_name_+"/joint_trajectory_action"); 00129 } 00130 } else { 00131 right_gripper_client_ = NULL; 00132 right_arm_trajectory_client_ = NULL; 00133 } 00134 if(control_larm_) { 00135 left_gripper_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>("l_gripper_controller/gripper_action", true); 00136 left_arm_trajectory_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>(l_arm_controller_name_+"/joint_trajectory_action", true); 00137 left_arm_traj_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(l_arm_controller_name_+"/command", 1); 00138 while(!left_gripper_client_->waitForServer(ros::Duration(5.0))){ 00139 ROS_INFO("Waiting for the right gripper action server to come up"); 00140 } 00141 while(!left_arm_trajectory_client_->waitForServer(ros::Duration(5.0))){ 00142 ROS_INFO_STREAM("Waiting for the left arm trajectory action server to come up"); 00143 } 00144 } else { 00145 left_gripper_client_ = NULL; 00146 left_arm_trajectory_client_ = NULL; 00147 } 00148 if(control_larm_ || control_rarm_) { 00149 tuck_arms_client_ = new actionlib::SimpleActionClient<pr2_common_action_msgs::TuckArmsAction>("tuck_arms", true); 00150 } else { 00151 tuck_arms_client_ = NULL; 00152 } 00153 00154 if(control_rarm_) { 00155 ROS_INFO("Waiting for right arm kinematics servers"); 00156 ros::service::waitForService("pr2_right_arm_kinematics/get_fk_solver_info"); 00157 ros::service::waitForService("pr2_right_arm_kinematics/get_fk"); 00158 ros::service::waitForService("pr2_right_arm_kinematics/get_ik"); 00159 right_arm_kinematics_solver_client_ = n_.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_right_arm_kinematics/get_fk_solver_info", true); 00160 right_arm_kinematics_forward_client_ = n_.serviceClient<kinematics_msgs::GetPositionFK>("pr2_right_arm_kinematics/get_fk", true); 00161 right_arm_kinematics_inverse_client_ = n_.serviceClient<kinematics_msgs::GetPositionIK>("pr2_right_arm_kinematics/get_ik", true); 00162 } 00163 00164 if(control_larm_) { 00165 ROS_INFO("Waiting for left arm kinematics servers"); 00166 ros::service::waitForService("pr2_left_arm_kinematics/get_fk"); 00167 ros::service::waitForService("pr2_left_arm_kinematics/get_fk_solver_info"); 00168 ros::service::waitForService("pr2_left_arm_kinematics/get_ik"); 00169 left_arm_kinematics_solver_client_ = n_.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("pr2_left_arm_kinematics/get_fk_solver_info", true); 00170 left_arm_kinematics_forward_client_ = n_.serviceClient<kinematics_msgs::GetPositionFK>("pr2_left_arm_kinematics/get_fk", true); 00171 left_arm_kinematics_inverse_client_ = n_.serviceClient<kinematics_msgs::GetPositionIK>("pr2_left_arm_kinematics/get_ik", true); 00172 } 00173 00174 00175 if(control_prosilica_) { 00176 ros::service::waitForService("prosilica/request_image"); 00177 prosilica_polling_client_ = n_.serviceClient<polled_camera::GetPolledImage>("prosilica/request_image", true); 00178 } 00179 00180 right_walk_along_pose_.push_back(.049); 00181 right_walk_along_pose_.push_back(-.116); 00182 right_walk_along_pose_.push_back(.036); 00183 right_walk_along_pose_.push_back(-1.272); 00184 right_walk_along_pose_.push_back(-.084); 00185 right_walk_along_pose_.push_back(-.148); 00186 right_walk_along_pose_.push_back(-0.027); 00187 00188 left_walk_along_pose_.push_back(0.0); 00189 left_walk_along_pose_.push_back(-.149); 00190 left_walk_along_pose_.push_back(.085); 00191 left_walk_along_pose_.push_back(-1.234); 00192 left_walk_along_pose_.push_back(0.030); 00193 left_walk_along_pose_.push_back(-.141); 00194 left_walk_along_pose_.push_back(3.114); 00195 00196 head_nod_traj_.joint_names.push_back("head_pan_joint"); 00197 head_nod_traj_.joint_names.push_back("head_tilt_joint"); 00198 head_nod_traj_.points.resize(3); 00199 head_nod_traj_.points[0].positions.push_back(0.0); 00200 head_nod_traj_.points[0].positions.push_back(-0.2); 00201 head_nod_traj_.points[0].velocities.push_back(0.0); 00202 head_nod_traj_.points[0].velocities.push_back(0.0); 00203 head_nod_traj_.points[0].time_from_start = ros::Duration(0.5); 00204 00205 head_nod_traj_.points[1].positions.push_back(0.0); 00206 head_nod_traj_.points[1].positions.push_back(0.2); 00207 head_nod_traj_.points[1].velocities.push_back(0.0); 00208 head_nod_traj_.points[1].velocities.push_back(0.0); 00209 head_nod_traj_.points[1].time_from_start = ros::Duration(1.0); 00210 00211 head_nod_traj_.points[2].positions.push_back(0.0); 00212 head_nod_traj_.points[2].positions.push_back(0.0); 00213 head_nod_traj_.points[2].velocities.push_back(0.0); 00214 head_nod_traj_.points[2].velocities.push_back(0.0); 00215 head_nod_traj_.points[2].time_from_start = ros::Duration(1.5); 00216 00217 head_shake_traj_.joint_names.push_back("head_pan_joint"); 00218 head_shake_traj_.joint_names.push_back("head_tilt_joint"); 00219 head_shake_traj_.points.resize(3); 00220 head_shake_traj_.points[0].positions.push_back(.62); 00221 head_shake_traj_.points[0].positions.push_back(0.0); 00222 head_shake_traj_.points[0].velocities.push_back(0.0); 00223 head_shake_traj_.points[0].velocities.push_back(0.0); 00224 head_shake_traj_.points[0].time_from_start = ros::Duration(0.5); 00225 00226 head_shake_traj_.points[1].positions.push_back(-.62); 00227 head_shake_traj_.points[1].positions.push_back(0.0); 00228 head_shake_traj_.points[1].velocities.push_back(0.0); 00229 head_shake_traj_.points[1].velocities.push_back(0.0); 00230 head_shake_traj_.points[1].time_from_start = ros::Duration(1.0); 00231 00232 head_shake_traj_.points[2].positions.push_back(0.0); 00233 head_shake_traj_.points[2].positions.push_back(0.0); 00234 head_shake_traj_.points[2].velocities.push_back(0.0); 00235 head_shake_traj_.points[2].velocities.push_back(0.0); 00236 head_shake_traj_.points[2].time_from_start = ros::Duration(1.5); 00237 00238 //making sure that everything is in the right mode 00239 right_arm_control_mode_ = ARM_MANNEQUIN_MODE; 00240 left_arm_control_mode_ = ARM_MANNEQUIN_MODE; 00241 setArmMode(ARMS_BOTH, ARM_POSITION_CONTROL); 00242 head_control_mode_ = HEAD_MANNEQUIN; 00243 if(control_head_) { 00244 setHeadMode(HEAD_JOYSTICK); 00245 laser_control_mode_ = LASER_TILT_OFF; 00246 } 00247 00248 //cribbed from motion planning laser settings 00249 laser_slow_period_ = 10; 00250 laser_slow_amplitude_ = .75; 00251 laser_slow_offset_ = .25; 00252 00253 //cribbed from head cart tilt_laser_launch 00254 laser_fast_period_ = 2; 00255 laser_fast_amplitude_ = .78; 00256 laser_fast_offset_ = .3; 00257 00258 last_right_wrist_goal_stamp_ = ros::Time::now(); 00259 last_left_wrist_goal_stamp_ = ros::Time::now(); 00260 00261 last_torso_vel_ = 0.0; 00262 walk_along_ok_ = false; 00263 } 00264 00265 GeneralCommander::~GeneralCommander() { 00266 if(head_track_hand_client_) { 00267 head_track_hand_client_->cancelAllGoals(); 00268 delete head_track_hand_client_; 00269 } 00270 if(right_gripper_client_) { 00271 delete right_gripper_client_; 00272 } 00273 if(left_gripper_client_) { 00274 delete left_gripper_client_; 00275 } 00276 if(right_arm_trajectory_client_) { 00277 delete right_arm_trajectory_client_; 00278 } 00279 if(left_arm_trajectory_client_) { 00280 delete left_arm_trajectory_client_; 00281 } 00282 if(tuck_arms_client_) { 00283 delete tuck_arms_client_; 00284 } 00285 } 00286 00287 void GeneralCommander::jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState) 00288 { 00289 for(unsigned int i = 0; i < jointState->name.size(); i++) { 00290 joint_state_position_map_[jointState->name[i]] = jointState->position[i]; 00291 joint_state_velocity_map_[jointState->name[i]] = jointState->velocity[i]; 00292 //if(jointState->name[i] == "r_wrist_roll_joint") { 00293 // ROS_INFO_STREAM("Right wrist roll pos " << jointState->position[i] << " vel " << jointState->velocity[i]); 00294 //} 00295 } 00296 } 00297 00298 bool GeneralCommander::getJointPosition(const std::string& name, double& pos) const { 00299 if(joint_state_position_map_.find(name) == joint_state_position_map_.end()) { 00300 return false; 00301 } 00302 pos = joint_state_position_map_.find(name)->second; 00303 return true; 00304 } 00305 00306 bool GeneralCommander::getJointVelocity(const std::string& name, double& vel) const { 00307 if(joint_state_velocity_map_.find(name) == joint_state_velocity_map_.end()) { 00308 return false; 00309 } 00310 vel = joint_state_velocity_map_.find(name)->second; 00311 return true; 00312 } 00313 00314 GeneralCommander::LaserControlMode GeneralCommander::getLaserMode() { 00315 return laser_control_mode_; 00316 } 00317 00318 GeneralCommander::HeadControlMode GeneralCommander::getHeadMode() { 00319 return head_control_mode_; 00320 } 00321 00322 GeneralCommander::ArmControlMode GeneralCommander::getArmMode(WhichArm arm) { 00323 if(arm == ARMS_RIGHT || arm == ARMS_BOTH) { 00324 return right_arm_control_mode_; 00325 } else { 00326 return left_arm_control_mode_; 00327 } 00328 } 00329 00330 void GeneralCommander::sendProjectorStartStop(bool start) { 00331 00332 if(!control_head_) return; 00333 00334 if(start) { 00335 int ok = system("rosrun dynamic_reconfigure dynparam set camera_synchronizer_node narrow_stereo_trig_mode 3"); 00336 ROS_DEBUG("Trying to send projector on"); 00337 if(ok < 0) { 00338 ROS_WARN("Dynamic reconfigure for setting trigger mode ON failed"); 00339 } 00340 } else { 00341 int ok = system("rosrun dynamic_reconfigure dynparam set camera_synchronizer_node narrow_stereo_trig_mode 4"); 00342 ROS_DEBUG("Trying to send trigger off"); 00343 if(ok < 0) { 00344 ROS_WARN("Dynamic reconfigure for setting trigger mode OFF failed"); 00345 } 00346 } 00347 } 00348 00349 void GeneralCommander::setLaserMode(LaserControlMode mode) { 00350 if(!control_head_) return; 00351 00352 if(laser_control_mode_ == mode) return; 00353 00354 pr2_msgs::SetPeriodicCmd::Request req; 00355 pr2_msgs::SetPeriodicCmd::Response res; 00356 00357 req.command.profile = "linear"; 00358 if(mode == LASER_TILT_SLOW) { 00359 ROS_DEBUG("Sending slow"); 00360 req.command.period = laser_slow_period_; 00361 req.command.amplitude = laser_slow_period_; 00362 req.command.offset = laser_slow_offset_; 00363 } else if(mode == LASER_TILT_FAST) { 00364 ROS_DEBUG("Sending fast"); 00365 req.command.period = laser_fast_period_; 00366 req.command.amplitude = laser_fast_period_; 00367 req.command.offset = laser_fast_offset_; 00368 } else { 00369 ROS_DEBUG("Sending off"); 00370 req.command.period = 1.0; 00371 req.command.amplitude = 0; 00372 req.command.offset = laser_slow_offset_; 00373 } 00374 00375 if(tilt_laser_service_.call(req,res)) { 00376 //ROS_DEBUG("Resp start time is %g", res.start_time.toSeconds()); 00377 } else { 00378 ROS_ERROR("Tilt laser service call failed.\n"); 00379 } 00380 laser_control_mode_ = mode; 00381 } 00382 00383 void GeneralCommander::setHeadMode(HeadControlMode mode) { 00384 if(!control_head_) return; 00385 if(mode == head_control_mode_) return; 00386 if(mode == HEAD_TRACK_LEFT_HAND) { 00387 ROS_DEBUG("Setting head to track left hand"); 00388 } else if(mode == HEAD_TRACK_RIGHT_HAND) { 00389 ROS_DEBUG("Setting head to track right hand"); 00390 } 00391 std::vector<std::string> start_controllers; 00392 std::vector<std::string> stop_controllers; 00393 if(mode == HEAD_MANNEQUIN) { 00394 start_controllers.push_back(HEAD_MANNEQUIN_CONTROLLER); 00395 stop_controllers.push_back(HEAD_POSITION_CONTROLLER); 00396 } else if(head_control_mode_ == HEAD_MANNEQUIN) { 00397 start_controllers.push_back(HEAD_POSITION_CONTROLLER); 00398 stop_controllers.push_back(HEAD_MANNEQUIN_CONTROLLER); 00399 } 00400 if(!start_controllers.empty() || !stop_controllers.empty()) { 00401 switchControllers(start_controllers, stop_controllers); 00402 } 00403 head_control_mode_ = mode; 00404 } 00405 00406 void GeneralCommander::setArmMode(WhichArm arm, ArmControlMode mode) { 00407 if(!control_rarm_ && !control_larm_) return; 00408 if(!control_rarm_ && arm == ARMS_RIGHT) return; 00409 if(!control_larm_ && arm == ARMS_LEFT) return; 00410 00411 if(arm == ARMS_LEFT) { 00412 if(mode == left_arm_control_mode_) return; 00413 } else if(arm == ARMS_RIGHT) { 00414 if(mode == right_arm_control_mode_) return; 00415 } else { 00416 if(mode == left_arm_control_mode_ && mode == right_arm_control_mode_) return; 00417 } 00418 00419 std::string left_running_controller; 00420 std::string right_running_controller; 00421 00422 if(left_arm_control_mode_ == ARM_MANNEQUIN_MODE) { 00423 left_running_controller = LEFT_ARM_MANNEQUIN_CONTROLLER; 00424 } else if(left_arm_control_mode_ == ARM_POSITION_CONTROL) { 00425 left_running_controller = l_arm_controller_name_; 00426 } 00427 00428 if(right_arm_control_mode_ == ARM_MANNEQUIN_MODE) { 00429 right_running_controller = RIGHT_ARM_MANNEQUIN_CONTROLLER; 00430 } else if(right_arm_control_mode_ == ARM_POSITION_CONTROL) { 00431 right_running_controller = r_arm_controller_name_; 00432 } 00433 00434 std::vector<std::string> start_controllers; 00435 std::vector<std::string> stop_controllers; 00436 00437 if(mode == ARM_NO_CONTROLLER) { 00438 if(arm == ARMS_LEFT || arm == ARMS_BOTH) { 00439 stop_controllers.push_back(left_running_controller); 00440 } 00441 if(arm == ARMS_RIGHT || arm == ARMS_BOTH) { 00442 stop_controllers.push_back(right_running_controller); 00443 } 00444 } else if(mode == ARM_MANNEQUIN_MODE) { 00445 if(arm == ARMS_LEFT || arm == ARMS_BOTH) { 00446 if(!left_running_controller.empty()) { 00447 stop_controllers.push_back(left_running_controller); 00448 } 00449 start_controllers.push_back(LEFT_ARM_MANNEQUIN_CONTROLLER); 00450 } 00451 if(arm == ARMS_RIGHT || arm == ARMS_BOTH) { 00452 if(!right_running_controller.empty()) { 00453 stop_controllers.push_back(right_running_controller); 00454 } 00455 start_controllers.push_back(RIGHT_ARM_MANNEQUIN_CONTROLLER); 00456 } 00457 } else if(mode == ARM_POSITION_CONTROL) { 00458 if(arm == ARMS_LEFT || arm == ARMS_BOTH) { 00459 if(!left_running_controller.empty()) { 00460 stop_controllers.push_back(left_running_controller); 00461 } 00462 start_controllers.push_back(l_arm_controller_name_); 00463 } 00464 if(arm == ARMS_RIGHT || arm == ARMS_BOTH) { 00465 if(!right_running_controller.empty()) { 00466 stop_controllers.push_back(right_running_controller); 00467 } 00468 start_controllers.push_back(r_arm_controller_name_); 00469 } 00470 } 00471 switchControllers(start_controllers, stop_controllers); 00472 if(arm == ARMS_LEFT || arm == ARMS_BOTH) { 00473 left_arm_control_mode_ = mode; 00474 } 00475 if(arm == ARMS_RIGHT || arm == ARMS_BOTH) { 00476 right_arm_control_mode_ = mode; 00477 } 00478 } 00479 00480 void GeneralCommander::sendHeadCommand(double req_pan, double req_tilt) { 00481 if(!control_head_) return; 00482 if(head_control_mode_ != HEAD_JOYSTICK) { 00483 return; 00484 } 00485 //TODO - correct 00486 trajectory_msgs::JointTrajectory traj; 00487 traj.header.stamp = ros::Time::now() + ros::Duration(0.01); 00488 traj.joint_names.push_back("head_pan_joint"); 00489 traj.joint_names.push_back("head_tilt_joint"); 00490 traj.points.resize(1); 00491 traj.points[0].positions.push_back(req_pan); 00492 traj.points[0].velocities.push_back(0.0);//req_pan_vel); 00493 traj.points[0].positions.push_back(req_tilt); 00494 traj.points[0].velocities.push_back(0.0);//req_tilt_vel); 00495 traj.points[0].time_from_start = ros::Duration(0.1); 00496 //ROS_INFO_STREAM("Publishing " << req_pan << " " << req_tilt); 00497 head_pub_.publish(traj); 00498 } 00499 00500 void GeneralCommander::sendHeadTrackCommand() { 00501 if(!control_head_) return; 00502 if(head_control_mode_ != HEAD_TRACK_LEFT_HAND && 00503 head_control_mode_ != HEAD_TRACK_RIGHT_HAND) { 00504 return; 00505 } 00506 00507 //the goal message we will be sending 00508 pr2_controllers_msgs::PointHeadGoal goal; 00509 00510 //the target point, expressed in the requested frame 00511 geometry_msgs::PointStamped point; 00512 if(head_control_mode_ == HEAD_TRACK_LEFT_HAND) { 00513 point.header.frame_id = LEFT_HAND_LINK_TO_TRACK; 00514 } else { 00515 point.header.frame_id = RIGHT_HAND_LINK_TO_TRACK; 00516 } 00517 point.point.x = 0.0; 00518 point.point.y = 0.0; 00519 point.point.z = 0.0; 00520 goal.target = point; 00521 00522 //we are pointing the high-def camera frame 00523 //(pointing_axis defaults to X-axis) 00524 goal.pointing_frame = "high_def_frame"; 00525 00526 //take at least 0.5 seconds to get there 00527 goal.min_duration = ros::Duration(0.1); 00528 00529 //and go no faster than 1 rad/s 00530 goal.max_velocity = MAX_HEAD_TRACK_SPEED; 00531 00532 //send the goal 00533 head_track_hand_client_->sendGoal(goal); 00534 00535 //TODO - preempty going to be problematic? 00536 00537 } 00538 00539 void GeneralCommander::sendGripperCommand(WhichArm which, bool close) { 00540 if(!control_rarm_ && !control_larm_) return; 00541 if(!control_rarm_ && which == ARMS_RIGHT) return; 00542 if(!control_larm_ && which == ARMS_LEFT) return; 00543 if(which == ARMS_RIGHT || which == ARMS_BOTH) { 00544 pr2_controllers_msgs::Pr2GripperCommandGoal com; 00545 if(close) { 00546 com.command.position = GRIPPER_CLOSE_POSITION; 00547 com.command.max_effort = GRIPPER_CLOSE_MAX_EFFORT; 00548 } else { 00549 com.command.position = GRIPPER_OPEN_POSITION; 00550 com.command.max_effort = GRIPPER_OPEN_MAX_EFFORT; 00551 } 00552 right_gripper_client_->sendGoal(com); 00553 right_gripper_client_->waitForResult(ros::Duration(5.0)); 00554 if(right_gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 00555 ROS_DEBUG("Right gripper command succeeded"); 00556 else 00557 ROS_WARN("Right gripper command failed"); 00558 } 00559 if(which == ARMS_LEFT || which == ARMS_BOTH) { 00560 pr2_controllers_msgs::Pr2GripperCommandGoal com; 00561 if(close) { 00562 com.command.position = GRIPPER_CLOSE_POSITION; 00563 com.command.max_effort = GRIPPER_CLOSE_MAX_EFFORT; 00564 } else { 00565 com.command.position = GRIPPER_OPEN_POSITION; 00566 com.command.max_effort = GRIPPER_OPEN_MAX_EFFORT; 00567 } 00568 left_gripper_client_->sendGoal(com); 00569 left_gripper_client_->waitForResult(ros::Duration(5.0)); 00570 if(left_gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 00571 ROS_DEBUG("Left gripper command succeeded"); 00572 else 00573 ROS_WARN("Left gripper command failed"); 00574 } 00575 00576 } 00577 00578 void GeneralCommander::sendTorsoCommand(double pos, double vel) { 00579 if(!control_body_) return; 00580 //only do this if we are commanding some velocity and not transitioning 00581 if(fabs(vel) < .0001 && fabs(last_torso_vel_ < .0001)) { 00582 //return; 00583 } 00584 last_torso_vel_ = vel; 00585 //ROS_INFO_STREAM("Torso pos " << pos << " vel " << vel); 00586 trajectory_msgs::JointTrajectory traj; 00587 traj.header.stamp = ros::Time::now() + ros::Duration(0.01); 00588 traj.joint_names.push_back("torso_lift_joint"); 00589 traj.points.resize(1); 00590 traj.points[0].positions.push_back(pos); 00591 traj.points[0].velocities.push_back(vel); 00592 traj.points[0].time_from_start = ros::Duration(0.25); 00593 torso_pub_.publish(traj); 00594 } 00595 00596 void GeneralCommander::sendBaseCommand(double vx, double vy, double vw) { 00597 if(!control_body_) return; 00598 geometry_msgs::Twist cmd; 00599 00600 cmd.linear.x = vx; 00601 cmd.linear.y = vy; 00602 cmd.angular.z = vw; 00603 base_pub_.publish(cmd); 00604 } 00605 00606 void GeneralCommander::switchControllers(const std::vector<std::string>& start_controllers, const std::vector<std::string>& stop_controllers) { 00607 pr2_mechanism_msgs::SwitchController::Request req; 00608 pr2_mechanism_msgs::SwitchController::Response res; 00609 req.start_controllers = start_controllers; 00610 req.stop_controllers = stop_controllers; 00611 for(std::vector<std::string>::const_iterator it = start_controllers.begin(); 00612 it != start_controllers.end(); 00613 it++) { 00614 ROS_DEBUG_STREAM("Trying to start controller " << (*it)); 00615 } 00616 for(std::vector<std::string>::const_iterator it = stop_controllers.begin(); 00617 it != stop_controllers.end(); 00618 it++) { 00619 ROS_DEBUG_STREAM("Trying to stop controller " << (*it)); 00620 } 00621 req.strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT; 00622 if(!switch_controllers_service_.call(req,res)) { 00623 ROS_WARN("Call to switch controllers failed entirely"); 00624 } 00625 if(res.ok != true) { 00626 ROS_WARN("Call to switch controllers reports not ok"); 00627 } 00628 } 00629 00630 void GeneralCommander::sendWristVelCommands(double right_wrist_vel, double left_wrist_vel, double hz) { 00631 00632 clampDesiredArmPositionsToActual(.05); 00633 00634 if(control_rarm_) { 00635 if(abs(right_wrist_vel) > .01) { 00636 if((ros::Time::now()-last_right_wrist_goal_stamp_).toSec() > .5) { 00637 clampDesiredArmPositionsToActual(0.0); 00638 } 00639 last_right_wrist_goal_stamp_ = ros::Time::now(); 00640 00641 //our goal variable 00642 pr2_controllers_msgs::JointTrajectoryGoal right_goal; 00643 composeWristRotGoal("r", right_goal, right_des_joint_states_ ,right_wrist_vel, hz); 00644 right_arm_traj_pub_.publish(right_goal.trajectory); 00645 //right_arm_trajectory_client_->sendGoal(right_goal); 00646 } 00647 } 00648 if(control_larm_) { 00649 if(abs(left_wrist_vel) > .01) { 00650 if((ros::Time::now()-last_left_wrist_goal_stamp_).toSec() > .5) { 00651 clampDesiredArmPositionsToActual(0.0); 00652 } 00653 last_left_wrist_goal_stamp_ = ros::Time::now(); 00654 //our goal variable 00655 pr2_controllers_msgs::JointTrajectoryGoal left_goal; 00656 composeWristRotGoal("l", left_goal, left_des_joint_states_, left_wrist_vel, hz); 00657 left_arm_traj_pub_.publish(left_goal.trajectory); 00658 //left_arm_trajectory_client_->sendGoal(left_goal); 00659 } 00660 } 00661 } 00662 00663 void GeneralCommander::composeWristRotGoal(const std::string pref, pr2_controllers_msgs::JointTrajectoryGoal& goal, 00664 std::vector<double>& des_joints, 00665 double des_vel, double hz) const { 00666 00667 double trajectory_duration = .2; 00668 00669 std::vector<std::string> joint_names; 00670 00671 // First, the joint names, which apply to all waypoints 00672 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 00673 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 00674 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 00675 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 00676 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 00677 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 00678 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 00679 00680 double end_pos_diff = (trajectory_duration)*des_vel; 00681 double cur_pos_diff = (1.0/hz)*des_vel; 00682 00683 goal.trajectory.joint_names=joint_names; 00684 goal.trajectory.points.resize(1); 00685 goal.trajectory.points[0].positions = des_joints; 00686 00687 goal.trajectory.points[0].velocities.resize(7,0.0); 00688 //goal.trajectory.points[0].velocities[6] = des_vel; 00689 00690 goal.trajectory.points[0].positions[6] += end_pos_diff; 00691 des_joints[6] += cur_pos_diff; 00692 //ROS_INFO_STREAM("Sending pos goal " << goal.trajectory.points[1].positions[6]); 00693 00694 goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.030); 00695 goal.trajectory.points[0].time_from_start = ros::Duration(trajectory_duration); 00696 } 00697 00698 void GeneralCommander::updateCurrentWristPositions() { 00699 00700 if(control_rarm_) { 00701 kinematics_msgs::GetKinematicSolverInfo::Request right_request; 00702 kinematics_msgs::GetKinematicSolverInfo::Response right_response; 00703 00704 kinematics_msgs::GetPositionFK::Request right_fk_request; 00705 kinematics_msgs::GetPositionFK::Response right_fk_response; 00706 00707 right_arm_kinematics_solver_client_.call(right_request, right_response); 00708 00709 right_fk_request.header.frame_id = "base_link"; 00710 right_fk_request.fk_link_names.push_back("r_wrist_roll_link"); 00711 right_fk_request.robot_state.joint_state.position.resize(right_response.kinematic_solver_info.joint_names.size()); 00712 right_fk_request.robot_state.joint_state.name = right_response.kinematic_solver_info.joint_names; 00713 for(unsigned int i = 0; i < right_fk_request.robot_state.joint_state.name.size(); i++) { 00714 bool ok = getJointPosition(right_fk_request.robot_state.joint_state.name[i], right_fk_request.robot_state.joint_state.position[i]); 00715 if(!ok) { 00716 ROS_WARN_STREAM("No joint state yet for " << right_fk_request.robot_state.joint_state.name[i]); 00717 return; 00718 } 00719 } 00720 if(right_arm_kinematics_forward_client_.call(right_fk_request, right_fk_response)) { 00721 if(right_fk_response.error_code.val == right_fk_response.error_code.SUCCESS) { 00722 right_wrist_roll_pose_ = right_fk_response.pose_stamped[0].pose; 00723 } else { 00724 ROS_DEBUG("Right fk not a success"); 00725 } 00726 } else { 00727 ROS_WARN("Right fk call failed all together"); 00728 } 00729 } 00730 00731 if(control_larm_) { 00732 kinematics_msgs::GetKinematicSolverInfo::Request left_request; 00733 kinematics_msgs::GetKinematicSolverInfo::Response left_response; 00734 00735 kinematics_msgs::GetPositionFK::Request left_fk_request; 00736 kinematics_msgs::GetPositionFK::Response left_fk_response; 00737 00738 left_arm_kinematics_solver_client_.call(left_request, left_response); 00739 00740 left_fk_request.header.frame_id = "base_link"; 00741 left_fk_request.fk_link_names.push_back("l_wrist_roll_link"); 00742 left_fk_request.robot_state.joint_state.position.resize(left_response.kinematic_solver_info.joint_names.size()); 00743 left_fk_request.robot_state.joint_state.name = left_response.kinematic_solver_info.joint_names; 00744 for(unsigned int i = 0; i < left_fk_request.robot_state.joint_state.name.size(); i++) { 00745 bool ok = getJointPosition(left_fk_request.robot_state.joint_state.name[i], left_fk_request.robot_state.joint_state.position[i]); 00746 if(!ok) { 00747 ROS_WARN_STREAM("No joint state yet for " << left_fk_request.robot_state.joint_state.name[i]); 00748 return; 00749 } 00750 } 00751 if(left_arm_kinematics_forward_client_.call(left_fk_request, left_fk_response)) { 00752 if(left_fk_response.error_code.val == left_fk_response.error_code.SUCCESS) { 00753 left_wrist_roll_pose_ = left_fk_response.pose_stamped[0].pose; 00754 } else { 00755 ROS_DEBUG("Left fk not a success"); 00756 } 00757 } else { 00758 ROS_WARN("Left fk call failed all together"); 00759 } 00760 00761 //ROS_INFO_STREAM("Current x " << left_wrist_roll_pose_.position.x << " y " << left_wrist_roll_pose_.position.y << " z " << left_wrist_roll_pose_.position.z); 00762 } 00763 } 00764 00765 void GeneralCommander::clampDesiredArmPositionsToActual(double max_dist) { 00766 00767 updateCurrentWristPositions(); 00768 00769 double tot_distance; 00770 if(control_rarm_) { 00771 tot_distance = sqrt(pow(des_r_wrist_roll_pose_.position.x-right_wrist_roll_pose_.position.x,2.0)+ 00772 pow(des_r_wrist_roll_pose_.position.y-right_wrist_roll_pose_.position.y,2.0)+ 00773 pow(des_r_wrist_roll_pose_.position.z-right_wrist_roll_pose_.position.z,2.0)); 00774 00775 //ROS_INFO_STREAM("Cur " << right_wrist_roll_pose_.position.x << " " << right_wrist_roll_pose_.position.y << " " << right_wrist_roll_pose_.position.z 00776 // << " des " << des_r_wrist_roll_pose_.position.x << " " << des_r_wrist_roll_pose_.position.y << " " << des_r_wrist_roll_pose_.position.z); 00777 00778 //ROS_INFO_STREAM("Total distance " << tot_distance); 00779 00780 if(tot_distance > max_dist) { 00781 des_r_wrist_roll_pose_ = right_wrist_roll_pose_; 00782 00783 std::vector<std::string> joint_names; 00784 std::string pref = "r"; 00785 // First, the joint names, which apply to all waypoints 00786 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 00787 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 00788 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 00789 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 00790 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 00791 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 00792 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 00793 right_des_joint_states_.clear(); 00794 for(unsigned int i = 0; i < joint_names.size(); i++) { 00795 double cur_pos; 00796 getJointPosition(joint_names[i], cur_pos); 00797 right_des_joint_states_.push_back(cur_pos); 00798 } 00799 //ROS_INFO("Clamping right"); 00800 } 00801 } 00802 00803 if(control_larm_) { 00804 tot_distance = sqrt(pow(des_l_wrist_roll_pose_.position.x-left_wrist_roll_pose_.position.x,2.0)+ 00805 pow(des_l_wrist_roll_pose_.position.y-left_wrist_roll_pose_.position.y,2.0)+ 00806 pow(des_l_wrist_roll_pose_.position.z-left_wrist_roll_pose_.position.z,2.0)); 00807 00808 if(tot_distance > max_dist) { 00809 des_l_wrist_roll_pose_ = left_wrist_roll_pose_; 00810 std::vector<std::string> joint_names; 00811 std::string pref = "l"; 00812 // First, the joint names, which apply to all waypoints 00813 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 00814 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 00815 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 00816 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 00817 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 00818 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 00819 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 00820 left_des_joint_states_.clear(); 00821 for(unsigned int i = 0; i < joint_names.size(); i++) { 00822 double cur_pos; 00823 getJointPosition(joint_names[i], cur_pos); 00824 left_des_joint_states_.push_back(cur_pos); 00825 } 00826 //ROS_INFO("Clamping left"); 00827 } 00828 } 00829 } 00830 00831 void GeneralCommander::unnormalizeTrajectory(trajectory_msgs::JointTrajectory& traj) const { 00832 00833 std::vector<double> current_values; 00834 std::vector<bool> wraparound; 00835 trajectory_msgs::JointTrajectory input_trajectory = traj; 00836 00837 for (size_t i=0; i<input_trajectory.joint_names.size(); i++) 00838 { 00839 std::string name = input_trajectory.joint_names[i]; 00840 00841 double pos; 00842 if(!getJointPosition(name, pos)) { 00843 ROS_WARN_STREAM("Can't unnormalize as no current joint state for " << name); 00844 return; 00845 } 00846 00847 //first waypoint is unnormalized relative to current joint states 00848 current_values.push_back(pos); 00849 00850 boost::shared_ptr<const urdf::Joint> joint = robot_model_.getJoint(name); 00851 if (joint.get() == NULL) 00852 { 00853 ROS_ERROR("Joint name %s not found in urdf model", name.c_str()); 00854 return; 00855 } 00856 if (joint->type == urdf::Joint::CONTINUOUS) { 00857 wraparound.push_back(true); 00858 } else { 00859 wraparound.push_back(false); 00860 } 00861 } 00862 00863 trajectory_msgs::JointTrajectory unnormalized_trajectory = input_trajectory; 00864 for (size_t i=0; i<unnormalized_trajectory.points.size(); i++) 00865 { 00866 for (size_t j=0; j<unnormalized_trajectory.points[i].positions.size(); j++) 00867 { 00868 if(!wraparound[j]){ 00869 continue; 00870 } 00871 double current = current_values[j]; 00872 double traj = unnormalized_trajectory.points[i].positions[j]; 00873 while ( current - traj > M_PI ) traj += 2*M_PI; 00874 while ( traj - current > M_PI ) traj -= 2*M_PI; 00875 ROS_DEBUG("Normalizing joint %s from %f to %f", unnormalized_trajectory.joint_names.at(j).c_str(), 00876 unnormalized_trajectory.points[i].positions[j], traj); 00877 unnormalized_trajectory.points[i].positions[j] = traj; 00878 //all other waypoints are unnormalized relative to the previous waypoint 00879 current_values[j] = traj; 00880 } 00881 } 00882 traj = unnormalized_trajectory; 00883 } 00884 00885 void GeneralCommander::sendArmVelCommands(double r_x_vel, double r_y_vel, double r_z_vel, double r_pitch_vel, double r_roll_vel, 00886 double l_x_vel, double l_y_vel, double l_z_vel, double l_pitch_vel, double l_roll_vel, 00887 double hz) { 00888 00889 00890 ros::Time beforeCall = ros::Time::now(); 00891 00892 double trajectory_duration = .2; 00893 00894 //ROS_INFO_STREAM("Got vels " << r_x_vel << " " << r_y_vel << " " << r_z_vel); 00895 00896 clampDesiredArmPositionsToActual(.1); 00897 00898 if(control_rarm_) { 00899 00900 if(fabs(r_x_vel) > .001 || fabs(r_y_vel) > .001 || fabs(r_z_vel) > .001 || fabs(r_roll_vel) > 0 || fabs(r_pitch_vel) > 0) { 00901 00902 geometry_msgs::Pose right_trajectory_endpoint = des_r_wrist_roll_pose_; 00903 00904 kinematics_msgs::GetKinematicSolverInfo::Request right_request; 00905 kinematics_msgs::GetKinematicSolverInfo::Response right_response; 00906 00907 right_arm_kinematics_solver_client_.call(right_request, right_response); 00908 00909 double pos_diff_x = r_x_vel*(1.0/hz);//*look_ahead; 00910 double pos_diff_y = r_y_vel*(1.0/hz);//*look_ahead; 00911 double pos_diff_z = r_z_vel*(1.0/hz);//*look_ahead; 00912 double pos_diff_pitch = r_pitch_vel*trajectory_duration; 00913 double pos_diff_roll = r_roll_vel*trajectory_duration; 00914 00915 right_trajectory_endpoint.position.x += r_x_vel*trajectory_duration; 00916 right_trajectory_endpoint.position.y += r_y_vel*trajectory_duration; 00917 right_trajectory_endpoint.position.z += r_z_vel*trajectory_duration; 00918 { 00919 urdf::Rotation q(right_trajectory_endpoint.orientation.x,right_trajectory_endpoint.orientation.y, 00920 right_trajectory_endpoint.orientation.z,right_trajectory_endpoint.orientation.w); 00921 double tmp[3]; 00922 q.getRPY(tmp[0],tmp[1],tmp[2]); 00923 tmp[0] += r_roll_vel*trajectory_duration; 00924 tmp[1] += r_pitch_vel*trajectory_duration; 00925 q.setFromRPY(tmp[0],tmp[1],tmp[2]); 00926 00927 right_trajectory_endpoint.orientation.x = q.x; 00928 right_trajectory_endpoint.orientation.y = q.y; 00929 right_trajectory_endpoint.orientation.z = q.z; 00930 right_trajectory_endpoint.orientation.w = q.w; 00931 } 00932 00933 des_r_wrist_roll_pose_.position.x += pos_diff_x; 00934 des_r_wrist_roll_pose_.position.y += pos_diff_y; 00935 des_r_wrist_roll_pose_.position.z += pos_diff_z; 00936 { 00937 urdf::Rotation q(des_r_wrist_roll_pose_.orientation.x,des_r_wrist_roll_pose_.orientation.y, 00938 des_r_wrist_roll_pose_.orientation.z,des_r_wrist_roll_pose_.orientation.w); 00939 double tmp[3]; 00940 q.getRPY(tmp[0],tmp[1],tmp[2]); 00941 tmp[0] += pos_diff_roll; 00942 tmp[1] += pos_diff_pitch; 00943 q.setFromRPY(tmp[0],tmp[1],tmp[2]); 00944 00945 des_r_wrist_roll_pose_.orientation.x = q.x; 00946 des_r_wrist_roll_pose_.orientation.y = q.y; 00947 des_r_wrist_roll_pose_.orientation.z = q.z; 00948 des_r_wrist_roll_pose_.orientation.w = q.w; 00949 } 00950 //ROS_INFO_STREAM("Desired " << des_r_wrist_roll_pose_.position.x << " " << des_r_wrist_roll_pose_.position.y << " " << des_r_wrist_roll_pose_.position.z); 00951 00952 //now call ik 00953 00954 kinematics_msgs::GetPositionIK::Request ik_req; 00955 kinematics_msgs::GetPositionIK::Response ik_res; 00956 00957 ik_req.timeout = ros::Duration(0.05); 00958 ik_req.ik_request.ik_link_name = "r_wrist_roll_link"; 00959 ik_req.ik_request.pose_stamped.header.frame_id = "base_link"; 00960 ik_req.ik_request.pose_stamped.header.stamp = ros::Time::now(); 00961 ik_req.ik_request.pose_stamped.pose = right_trajectory_endpoint; 00962 00963 ik_req.ik_request.ik_seed_state.joint_state.position.resize(right_response.kinematic_solver_info.joint_names.size()); 00964 ik_req.ik_request.ik_seed_state.joint_state.name = right_response.kinematic_solver_info.joint_names; 00965 for(unsigned int i = 0; i < ik_req.ik_request.ik_seed_state.joint_state.name.size(); i++) { 00966 bool ok = getJointPosition(ik_req.ik_request.ik_seed_state.joint_state.name[i], ik_req.ik_request.ik_seed_state.joint_state.position[i]); 00967 if(!ok) { 00968 ROS_WARN_STREAM("No joint state yet for " << ik_req.ik_request.ik_seed_state.joint_state.name[i]); 00969 return; 00970 } else { 00971 //ROS_INFO_STREAM("Setting position " << ik_req.ik_request.ik_seed_state.joint_state.position[i] << " For name " 00972 // << ik_req.ik_request.ik_seed_state.joint_state.name[i]); 00973 } 00974 } 00975 00976 00977 //otherwise not a lot to be done 00978 if(right_arm_kinematics_inverse_client_.call(ik_req, ik_res) && ik_res.error_code.val == ik_res.error_code.SUCCESS) { 00979 //ROS_INFO("Ik succeeded"); 00980 pr2_controllers_msgs::JointTrajectoryGoal goal; 00981 00982 std::vector<std::string> joint_names; 00983 std::string pref = "r"; 00984 00985 // First, the joint names, which apply to all waypoints 00986 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 00987 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 00988 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 00989 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 00990 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 00991 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 00992 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 00993 00994 goal.trajectory.joint_names = joint_names; 00995 00996 // We will have two waypoints in this goal trajectory 00997 goal.trajectory.points.resize(1); 00998 00999 for(unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { 01000 goal.trajectory.points[0].positions.push_back(ik_res.solution.joint_state.position[i]); 01001 goal.trajectory.points[0].velocities.push_back(0.0); 01002 } 01003 goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.200); 01004 goal.trajectory.points[0].time_from_start = ros::Duration(trajectory_duration); 01005 01006 unnormalizeTrajectory(goal.trajectory); 01007 right_arm_traj_pub_.publish(goal.trajectory); 01008 } else { 01009 ROS_DEBUG_STREAM("Ik failed with response " << ik_res.error_code.val); 01010 } 01011 } 01012 01013 ros::Time afterCall = ros::Time::now(); 01014 } 01015 if(control_larm_) { 01016 if(fabs(l_x_vel) > .001 || fabs(l_y_vel) > .001 || fabs(l_z_vel) > .001 || fabs(l_roll_vel) > 0 || fabs(l_pitch_vel) > 0) { 01017 01018 geometry_msgs::Pose left_trajectory_endpoint = des_l_wrist_roll_pose_; 01019 01020 kinematics_msgs::GetKinematicSolverInfo::Request left_request; 01021 kinematics_msgs::GetKinematicSolverInfo::Response left_response; 01022 01023 left_arm_kinematics_solver_client_.call(left_request, left_response); 01024 01025 double pos_diff_x = l_x_vel*(1.0/hz); 01026 double pos_diff_y = l_y_vel*(1.0/hz); 01027 double pos_diff_z = l_z_vel*(1.0/hz); 01028 double pos_diff_pitch = l_pitch_vel*trajectory_duration; 01029 double pos_diff_roll = l_roll_vel*trajectory_duration; 01030 01031 left_trajectory_endpoint.position.x += l_x_vel*trajectory_duration; 01032 left_trajectory_endpoint.position.y += l_y_vel*trajectory_duration; 01033 left_trajectory_endpoint.position.z += l_z_vel*trajectory_duration; 01034 01035 { 01036 urdf::Rotation q(left_trajectory_endpoint.orientation.x,left_trajectory_endpoint.orientation.y, 01037 left_trajectory_endpoint.orientation.z,left_trajectory_endpoint.orientation.w); 01038 double tmp[3]; 01039 q.getRPY(tmp[0],tmp[1],tmp[2]); 01040 tmp[0] += r_roll_vel*trajectory_duration; 01041 tmp[1] += r_pitch_vel*trajectory_duration; 01042 q.setFromRPY(tmp[0],tmp[1],tmp[2]); 01043 01044 left_trajectory_endpoint.orientation.x = q.x; 01045 left_trajectory_endpoint.orientation.y = q.y; 01046 left_trajectory_endpoint.orientation.z = q.z; 01047 left_trajectory_endpoint.orientation.w = q.w; 01048 } 01049 01050 des_l_wrist_roll_pose_.position.x += pos_diff_x; 01051 des_l_wrist_roll_pose_.position.y += pos_diff_y; 01052 des_l_wrist_roll_pose_.position.z += pos_diff_z; 01053 { 01054 urdf::Rotation q(des_l_wrist_roll_pose_.orientation.x,des_l_wrist_roll_pose_.orientation.y, 01055 des_l_wrist_roll_pose_.orientation.z,des_l_wrist_roll_pose_.orientation.w); 01056 double tmp[3]; 01057 q.getRPY(tmp[0],tmp[1],tmp[2]); 01058 tmp[0] += pos_diff_roll; 01059 tmp[1] += pos_diff_pitch; 01060 q.setFromRPY(tmp[0],tmp[1],tmp[2]); 01061 01062 des_l_wrist_roll_pose_.orientation.x = q.x; 01063 des_l_wrist_roll_pose_.orientation.y = q.y; 01064 des_l_wrist_roll_pose_.orientation.z = q.z; 01065 des_l_wrist_roll_pose_.orientation.w = q.w; 01066 } 01067 01068 //now call ik 01069 01070 kinematics_msgs::GetPositionIK::Request ik_req; 01071 kinematics_msgs::GetPositionIK::Response ik_res; 01072 01073 ik_req.timeout = ros::Duration(0.05); 01074 ik_req.ik_request.ik_link_name = "l_wrist_roll_link"; 01075 ik_req.ik_request.pose_stamped.header.frame_id = "base_link"; 01076 ik_req.ik_request.pose_stamped.header.stamp = ros::Time::now(); 01077 ik_req.ik_request.pose_stamped.pose = left_trajectory_endpoint; 01078 01079 ik_req.ik_request.ik_seed_state.joint_state.position.resize(left_response.kinematic_solver_info.joint_names.size()); 01080 ik_req.ik_request.ik_seed_state.joint_state.name = left_response.kinematic_solver_info.joint_names; 01081 for(unsigned int i = 0; i < ik_req.ik_request.ik_seed_state.joint_state.name.size(); i++) { 01082 bool ok = getJointPosition(ik_req.ik_request.ik_seed_state.joint_state.name[i], ik_req.ik_request.ik_seed_state.joint_state.position[i]); 01083 if(!ok) { 01084 ROS_WARN_STREAM("No joint state yet for " << ik_req.ik_request.ik_seed_state.joint_state.name[i]); 01085 return; 01086 } else { 01087 //ROS_INFO_STREAM("Setting position " << ik_req.ik_request.ik_seed_state.joint_state.position[i] << " For name " 01088 // << ik_req.ik_request.ik_seed_state.joint_state.name[i]); 01089 } 01090 } 01091 01092 01093 //otherwise not a lot to be done 01094 if(left_arm_kinematics_inverse_client_.call(ik_req, ik_res) && ik_res.error_code.val == ik_res.error_code.SUCCESS) { 01095 //ROS_INFO("Ik succeeded"); 01096 pr2_controllers_msgs::JointTrajectoryGoal goal; 01097 01098 std::vector<std::string> joint_names; 01099 std::string pref = "l"; 01100 01101 // First, the joint names, which apply to all waypoints 01102 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 01103 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 01104 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 01105 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 01106 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 01107 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 01108 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 01109 01110 goal.trajectory.joint_names = joint_names; 01111 01112 // We will have one waypoint in this goal trajectory 01113 goal.trajectory.points.resize(1); 01114 01115 for(unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { 01116 goal.trajectory.points[0].positions.push_back(ik_res.solution.joint_state.position[i]); 01117 goal.trajectory.points[0].velocities.push_back(0.0); 01118 } 01119 goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.200); 01120 goal.trajectory.points[0].time_from_start = ros::Duration(trajectory_duration); 01121 01122 unnormalizeTrajectory(goal.trajectory); 01123 left_arm_traj_pub_.publish(goal.trajectory); 01124 } else { 01125 ROS_DEBUG_STREAM("Ik failed with response " << ik_res.error_code.val); 01126 } 01127 } 01128 } 01129 } 01130 01131 bool GeneralCommander::moveToWalkAlongArmPose() { 01132 01133 //not implementing for one-armed robots 01134 if(!control_rarm_ || !control_larm_) { 01135 return false; 01136 } 01137 01138 updateCurrentWristPositions(); 01139 01140 ROS_DEBUG("Attempting to set arms to walk along pose"); 01141 01142 std::vector<std::string> joint_names; 01143 std::string pref; 01144 pr2_controllers_msgs::JointTrajectoryGoal goal; 01145 01146 pref = "r"; 01147 01148 // First, the joint names, which apply to all waypoints 01149 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 01150 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 01151 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 01152 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 01153 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 01154 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 01155 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 01156 01157 goal.trajectory.joint_names = joint_names; 01158 01159 // We will have one waypoint in this goal trajectory 01160 goal.trajectory.points.resize(1); 01161 goal.trajectory.points[0].positions = right_walk_along_pose_; 01162 01163 for(unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { 01164 goal.trajectory.points[0].velocities.push_back(0.0); 01165 } 01166 01167 geometry_msgs::Pose right_walk_pose = getPositionFromJointsPose(right_arm_kinematics_forward_client_, 01168 "r_wrist_roll_link", 01169 joint_names, 01170 right_walk_along_pose_); 01171 double tot_distance = sqrt(pow(right_walk_pose.position.x-right_wrist_roll_pose_.position.x,2.0)+ 01172 pow(right_walk_pose.position.y-right_wrist_roll_pose_.position.y,2.0)+ 01173 pow(right_walk_pose.position.z-right_wrist_roll_pose_.position.z,2.0)); 01174 ROS_DEBUG_STREAM("Right dist is " << tot_distance); 01175 01176 if(tot_distance > .02) { 01177 01178 goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.030); 01179 goal.trajectory.points[0].time_from_start = ros::Duration(3.0); 01180 01181 unnormalizeTrajectory(goal.trajectory); 01182 01183 ROS_DEBUG("Sending right arm goal"); 01184 01185 right_arm_trajectory_client_->sendGoal(goal); 01186 bool finished_before_timeout = right_arm_trajectory_client_->waitForResult(ros::Duration(5.0)); 01187 01188 actionlib::SimpleClientGoalState state = right_arm_trajectory_client_->getState(); 01189 01190 if(!finished_before_timeout || state != actionlib::SimpleClientGoalState::SUCCEEDED) { 01191 right_arm_trajectory_client_->sendGoal(goal); 01192 finished_before_timeout = right_arm_trajectory_client_->waitForResult(ros::Duration(5.0)); 01193 state = right_arm_trajectory_client_->getState(); 01194 if(!finished_before_timeout || state != actionlib::SimpleClientGoalState::SUCCEEDED) { 01195 ROS_WARN("Not in walk along pose"); 01196 return false; 01197 } 01198 } else { 01199 ROS_DEBUG("Right arm goal successful"); 01200 } 01201 } else { 01202 ROS_DEBUG("No need for right arm goal"); 01203 } 01204 joint_names.clear(); 01205 pref = "l"; 01206 01207 // First, the joint names, which apply to all waypoints 01208 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 01209 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 01210 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 01211 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 01212 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 01213 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 01214 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 01215 01216 goal.trajectory.joint_names = joint_names; 01217 goal.trajectory.points[0].positions = left_walk_along_pose_; 01218 goal.trajectory.points[0].velocities.clear(); 01219 for(unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) { 01220 goal.trajectory.points[0].velocities.push_back(0.0); 01221 } 01222 01223 geometry_msgs::Pose left_walk_pose = getPositionFromJointsPose(left_arm_kinematics_forward_client_, 01224 "l_wrist_roll_link", 01225 joint_names, 01226 left_walk_along_pose_); 01227 tot_distance = sqrt(pow(left_walk_pose.position.x-left_wrist_roll_pose_.position.x,2.0)+ 01228 pow(left_walk_pose.position.y-left_wrist_roll_pose_.position.y,2.0)+ 01229 pow(left_walk_pose.position.z-left_wrist_roll_pose_.position.z,2.0)); 01230 ROS_DEBUG_STREAM("Left dist is " << tot_distance); 01231 01232 if(tot_distance > .02) { 01233 01234 //goal.trajectory.points[0].positions[3] = 1.8; 01235 01236 goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.030); 01237 goal.trajectory.points[0].time_from_start = ros::Duration(3.0); 01238 01239 unnormalizeTrajectory(goal.trajectory); 01240 01241 ROS_DEBUG("Sending left arm goal"); 01242 01243 left_arm_trajectory_client_->sendGoal(goal); 01244 bool finished_before_timeout = left_arm_trajectory_client_->waitForResult(ros::Duration(5.0)); 01245 01246 actionlib::SimpleClientGoalState state = left_arm_trajectory_client_->getState(); 01247 01248 if(!finished_before_timeout || state != actionlib::SimpleClientGoalState::SUCCEEDED) { 01249 left_arm_trajectory_client_->sendGoal(goal); 01250 finished_before_timeout = left_arm_trajectory_client_->waitForResult(ros::Duration(5.0)); 01251 state = left_arm_trajectory_client_->getState(); 01252 if(!finished_before_timeout || state != actionlib::SimpleClientGoalState::SUCCEEDED) { 01253 ROS_WARN("Not in walk along pose"); 01254 return false; 01255 } 01256 } else { 01257 ROS_DEBUG("Left arm goal successful"); 01258 } 01259 } else { 01260 ROS_DEBUG("No need for arm goal"); 01261 } 01262 01263 //ros::Duration(1.0).sleep(); 01264 01265 updateCurrentWristPositions(); 01266 walk_along_right_des_pose_ = right_wrist_roll_pose_; 01267 walk_along_left_des_pose_ = left_wrist_roll_pose_; 01268 //ROS_INFO_STREAM("walk right " << walk_along_right_des_pose_.position.x << " " << walk_along_right_des_pose_.position.y << " " << walk_along_right_des_pose_.position.z); 01269 //ROS_INFO_STREAM("walk left " << walk_along_left_des_pose_.position.x << " " << walk_along_left_des_pose_.position.y << " " << walk_along_left_des_pose_.position.z); 01270 01271 walk_rdx_vals_.clear(); 01272 walk_rdy_vals_.clear(); 01273 walk_ldx_vals_.clear(); 01274 walk_ldy_vals_.clear(); 01275 01276 return true; 01277 } 01278 01279 bool GeneralCommander::initWalkAlong() { 01280 01281 //not implementing for one-armed robots 01282 if(!control_rarm_ || !control_larm_) { 01283 return false; 01284 } 01285 01286 updateCurrentWristPositions(); 01287 01288 std::vector<std::string> joint_names; 01289 std::string pref = "r"; 01290 01291 // First, the joint names, which apply to all waypoints 01292 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 01293 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 01294 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 01295 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 01296 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 01297 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 01298 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 01299 01300 geometry_msgs::Pose right_walk_pose = getPositionFromJointsPose(right_arm_kinematics_forward_client_, 01301 "r_wrist_roll_link", 01302 joint_names, 01303 right_walk_along_pose_); 01304 double tot_distance = sqrt(pow(right_walk_pose.position.x-right_wrist_roll_pose_.position.x,2.0)+ 01305 pow(right_walk_pose.position.y-right_wrist_roll_pose_.position.y,2.0)+ 01306 pow(right_walk_pose.position.z-right_wrist_roll_pose_.position.z,2.0)); 01307 ROS_DEBUG_STREAM("Right dist is " << tot_distance); 01308 01309 if(tot_distance > .02) { 01310 walk_along_ok_ = false; 01311 return false; 01312 } 01313 01314 joint_names.clear(); 01315 pref = "l"; 01316 01317 // First, the joint names, which apply to all waypoints 01318 joint_names.push_back(pref+"_"+"shoulder_pan_joint"); 01319 joint_names.push_back(pref+"_"+"shoulder_lift_joint"); 01320 joint_names.push_back(pref+"_"+"upper_arm_roll_joint"); 01321 joint_names.push_back(pref+"_"+"elbow_flex_joint"); 01322 joint_names.push_back(pref+"_"+"forearm_roll_joint"); 01323 joint_names.push_back(pref+"_"+"wrist_flex_joint"); 01324 joint_names.push_back(pref+"_"+"wrist_roll_joint"); 01325 geometry_msgs::Pose left_walk_pose = getPositionFromJointsPose(left_arm_kinematics_forward_client_, 01326 "l_wrist_roll_link", 01327 joint_names, 01328 left_walk_along_pose_); 01329 tot_distance = sqrt(pow(left_walk_pose.position.x-left_wrist_roll_pose_.position.x,2.0)+ 01330 pow(left_walk_pose.position.y-left_wrist_roll_pose_.position.y,2.0)+ 01331 pow(left_walk_pose.position.z-left_wrist_roll_pose_.position.z,2.0)); 01332 ROS_DEBUG_STREAM("Left dist is " << tot_distance); 01333 01334 if(tot_distance > .02) { 01335 walk_along_ok_ = false; 01336 return false; 01337 } 01338 walk_along_ok_ = true; 01339 return true; 01340 } 01341 01342 void GeneralCommander::updateWalkAlongAverages() { 01343 01344 //not implementing for one-armed robots 01345 if(!control_rarm_ || !control_larm_) { 01346 return; 01347 } 01348 01349 if(walk_rdx_vals_.size() > WALK_BUFFER) { 01350 walk_rdx_vals_.pop_front(); 01351 } 01352 if(walk_rdy_vals_.size() > WALK_BUFFER) { 01353 walk_rdy_vals_.pop_front(); 01354 } 01355 if(walk_ldx_vals_.size() > WALK_BUFFER) { 01356 walk_ldx_vals_.pop_front(); 01357 } 01358 if(walk_ldy_vals_.size() > WALK_BUFFER) { 01359 walk_ldy_vals_.pop_front(); 01360 } 01361 01362 updateCurrentWristPositions(); 01363 01364 //ROS_INFO_STREAM("Current right " << right_wrist_roll_pose_.position.x << " " << right_wrist_roll_pose_.position.y << " " << right_wrist_roll_pose_.position.z); 01365 //ROS_INFO_STREAM("Current left " << left_wrist_roll_pose_.position.x << " " << left_wrist_roll_pose_.position.y << " " << left_wrist_roll_pose_.position.z); 01366 01367 double rdx = right_wrist_roll_pose_.position.x-walk_along_right_des_pose_.position.x; 01368 double rdy = right_wrist_roll_pose_.position.y-walk_along_right_des_pose_.position.y; 01369 01370 double ldx = left_wrist_roll_pose_.position.x-walk_along_left_des_pose_.position.x; 01371 double ldy = left_wrist_roll_pose_.position.y-walk_along_left_des_pose_.position.y; 01372 01373 //ROS_INFO_STREAM(rdx << " " << rdy << " " << ldx << " " << ldy); 01374 01375 walk_rdx_vals_.push_back(rdx); 01376 walk_rdy_vals_.push_back(rdy); 01377 walk_ldx_vals_.push_back(ldx); 01378 walk_ldy_vals_.push_back(ldy); 01379 01380 } 01381 01382 void GeneralCommander::sendWalkAlongCommand(double thresh, 01383 double x_dist_max, double x_speed_scale, 01384 double y_dist_max, double y_speed_scale, 01385 double rot_speed_scale) { 01386 //not implementing for one-armed robots 01387 if(!control_rarm_ || !control_larm_) { 01388 return; 01389 } 01390 01391 if(!walk_along_ok_) { 01392 return; 01393 } 01394 01395 updateWalkAlongAverages(); 01396 01397 double av_rdx = calcAverage(walk_rdx_vals_); 01398 double av_rdy = calcAverage(walk_rdy_vals_); 01399 double av_ldx = calcAverage(walk_ldx_vals_); 01400 double av_ldy = calcAverage(walk_ldy_vals_); 01401 01402 //ROS_INFO_STREAM(av_rdx << " " << av_rdy << " " << av_ldx << " " << av_ldy); 01403 01404 if(fabs(av_rdx) < thresh) { 01405 av_rdx = 0.0; 01406 } 01407 if(fabs(av_rdy) < thresh) { 01408 av_rdy = 0.0; 01409 } 01410 if(fabs(av_ldx) < thresh) { 01411 av_ldx = 0.0; 01412 } 01413 if(fabs(av_ldy) < thresh) { 01414 av_ldy = 0.0; 01415 } 01416 01417 double vx = 0.0; 01418 double vy = 0.0; 01419 double vw = 0.0; 01420 01421 // if(ldx < -.0001 && rdx > .0001) { 01422 // vw = (fabs(rdx)+fabs(ldx))*rot_scale; 01423 // } else if(ldx > .0001 && rdx < -.0001) { 01424 // vw = (fabs(rdx)+fabs(ldx))*(-rot_scale); 01425 // } else { 01426 // vx = xy_scale*(fmax(fabs(rdx), fabs(ldx)))*((rdx > .001 || ldx > .001) ? 1.0 : -1.0); 01427 // vy = xy_scale*(fmax(fabs(rdy), fabs(ldy)))*((rdy > .001 || ldy > .001) ? 1.0 : -1.0); 01428 // } 01429 //if(fabs(vx) > .0001 || fabs(vy) > .0001 || fabs(vw) > .0001) { 01430 //ROS_INFO_STREAM("Walk along sending " << vx << " " << vy << " " << vw << " scale " << xy_scale); 01431 //} 01432 //sendBaseCommand(vx, vy, vw); 01433 01434 double av_x = av_rdx/2.0+av_ldx/2.0; 01435 double per_x = fabs(av_x)/x_dist_max; 01436 per_x = std::min(per_x, 1.0); 01437 vx = (per_x*per_x)*x_speed_scale*((av_x > 0) ? 1 : -1); 01438 01439 double per_y = fabs(av_ldy/2.0)/y_dist_max; 01440 per_y = std::min(per_y, 1.0); 01441 vy = (per_y*per_y)*y_speed_scale*((av_ldy > 0) ? 1 : -1); 01442 01443 //vx = xy_scale*((av_rdx+av_ldx)/2.0);//((av_rdx, fabs(av_ldx)))*((av_rdx > .001 || av_ldx > .001) ? 1.0 : -1.0); 01444 //vy = av_ldy*xy_scale; 01445 double per_rot = fabs(av_rdy/2.0)/y_dist_max; 01446 per_rot = std::min(per_rot, 1.0); 01447 vw = (per_rot*per_rot)*rot_speed_scale*((av_rdy > 0) ? 1 : -1);; 01448 01449 //ROS_INFO_STREAM("Walk along sending " << vx << " " << vy << " " << vw); 01450 01451 sendBaseCommand(vx, vy, vw); 01452 01453 //Eric's hacking below this line 01454 // vx = ldx * 7.0 + rdx * 2; 01455 // vy = ldy * 7.0 + rdy * 2 - rdx * 3; 01456 // vw = rdx * 12.0; 01457 // sendBaseCommand(vx, vy, vw); 01458 } 01459 01460 double GeneralCommander::calcAverage(const std::list<double>& av_list) const { 01461 double av = 0.0; 01462 for(std::list<double>::const_iterator it = av_list.begin(); 01463 it != av_list.end(); 01464 it++) { 01465 av += (*it); 01466 } 01467 av /= av_list.size(); 01468 return av; 01469 } 01470 01471 geometry_msgs::Pose GeneralCommander::getPositionFromJointsPose(ros::ServiceClient& service_client, 01472 std::string fk_link, 01473 const std::vector<std::string>& joint_names, const std::vector<double>& joint_pos) { 01474 kinematics_msgs::GetPositionFK::Request fk_request; 01475 kinematics_msgs::GetPositionFK::Response fk_response; 01476 01477 geometry_msgs::Pose ret_pose; 01478 01479 fk_request.header.frame_id = "base_link"; 01480 fk_request.fk_link_names.push_back(fk_link); 01481 fk_request.robot_state.joint_state.position.resize(joint_names.size()); 01482 fk_request.robot_state.joint_state.name = joint_names; 01483 fk_request.robot_state.joint_state.position = joint_pos; 01484 if(service_client.call(fk_request, fk_response)) { 01485 if(fk_response.error_code.val == fk_response.error_code.SUCCESS) { 01486 ret_pose = fk_response.pose_stamped[0].pose; 01487 } else { 01488 ROS_DEBUG("fk not a success"); 01489 } 01490 } else { 01491 ROS_WARN("fk call failed all together"); 01492 } 01493 return ret_pose; 01494 } 01495 01496 void GeneralCommander::sendHeadSequence(HeadSequence seq) { 01497 01498 if(!control_head_) return; 01499 01500 setHeadMode(HEAD_JOYSTICK); 01501 01502 trajectory_msgs::JointTrajectory traj; 01503 01504 if(seq == HEAD_NOD) { 01505 traj = head_nod_traj_; 01506 } else if(seq == HEAD_SHAKE){ 01507 traj = head_shake_traj_; 01508 } 01509 traj.header.stamp = ros::Time::now() + ros::Duration(0.01); 01510 head_pub_.publish(traj); 01511 } 01512 01513 void GeneralCommander::powerBoardCallback(const pr2_msgs::PowerBoardStateConstPtr &powerBoardState) { 01514 if(walk_along_ok_) { 01515 if(!powerBoardState->run_stop || !powerBoardState->wireless_stop) { 01516 ROS_DEBUG("Killing walk along due to stop"); 01517 walk_along_ok_ = false; 01518 } 01519 } 01520 } 01521 01522 void GeneralCommander::requestProsilicaImage(std::string ns) { 01523 if(!control_prosilica_) return; 01524 polled_camera::GetPolledImage::Request gpi_req; 01525 polled_camera::GetPolledImage::Response gpi_res; 01526 gpi_req.response_namespace = ns; 01527 if(!prosilica_polling_client_.call(gpi_req, gpi_res)) { 01528 ROS_WARN("Prosilica polling request failed"); 01529 } 01530 } 01531 01532 void GeneralCommander::tuckArms(WhichArm arm) { 01533 01534 //can't tuck just one arm for now 01535 if(!control_rarm_ || !control_larm_) { 01536 return; 01537 } 01538 01539 setArmMode(arm, ARM_POSITION_CONTROL); 01540 01541 pr2_common_action_msgs::TuckArmsGoal tuck_arm_goal; 01542 01543 if(arm == ARMS_BOTH) { 01544 tuck_arm_goal.tuck_right = true; 01545 tuck_arm_goal.tuck_left = true; 01546 } else { 01547 ROS_DEBUG("Tucking one arm not supported"); 01548 } 01549 01550 ROS_DEBUG("Sending tuck arms"); 01551 01552 tuck_arms_client_->sendGoalAndWait(tuck_arm_goal, ros::Duration(10.0), ros::Duration(5.0)); 01553 } 01554 01555 void GeneralCommander::untuckArms(WhichArm arm) { 01556 01557 //can't tuck just one arm for now 01558 if(!control_rarm_ || !control_larm_) { 01559 return; 01560 } 01561 01562 setArmMode(arm, ARM_POSITION_CONTROL); 01563 01564 pr2_common_action_msgs::TuckArmsGoal tuck_arm_goal; 01565 01566 if(arm == ARMS_BOTH) { 01567 tuck_arm_goal.tuck_right = false; 01568 tuck_arm_goal.tuck_left = false; 01569 } else { 01570 ROS_DEBUG("Untucking one arm not supported"); 01571 } 01572 01573 ROS_DEBUG("Sending untuck arms"); 01574 01575 tuck_arms_client_->sendGoalAndWait(tuck_arm_goal, ros::Duration(10.0), ros::Duration(5.0)); 01576 01577 }