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


pr2_teleop_general
Author(s): Gil Jones
autogenerated on Thu Jun 6 2019 21:08:11