$search
00001 /* 00002 * pr2_teleop_general_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 <cstdio> 00034 #include <cstdlib> 00035 #include <unistd.h> 00036 #include <math.h> 00037 00038 #include <ros/ros.h> 00039 #include <sensor_msgs/Joy.h> 00040 00041 #include <sensor_msgs/JointState.h> 00042 #include <pr2_msgs/SetPeriodicCmd.h> 00043 #include <geometry_msgs/Pose.h> 00044 #include <urdf/model.h> 00045 #include <actionlib/client/simple_action_client.h> 00046 #include <pr2_controllers_msgs/PointHeadAction.h> 00047 #include <pr2_controllers_msgs/Pr2GripperCommandAction.h> 00048 #include <pr2_controllers_msgs/JointTrajectoryAction.h> 00049 #include <pr2_common_action_msgs/TuckArmsAction.h> 00050 #include <pr2_msgs/PowerBoardState.h> 00051 00052 static const std::string default_arm_controller_name="arm_controller"; 00053 00054 class GeneralCommander { 00055 00056 public: 00057 00058 enum WhichArm { 00059 ARMS_LEFT, 00060 ARMS_RIGHT, 00061 ARMS_BOTH 00062 }; 00063 00064 enum ArmControlMode{ 00065 ARM_NO_CONTROLLER, 00066 ARM_MANNEQUIN_MODE, 00067 ARM_POSITION_CONTROL 00068 }; 00069 00070 enum HeadControlMode { 00071 HEAD_JOYSTICK, 00072 HEAD_TRACK_LEFT_HAND, 00073 HEAD_TRACK_RIGHT_HAND, 00074 HEAD_MANNEQUIN 00075 }; 00076 00077 enum LaserControlMode { 00078 LASER_TILT_OFF, 00079 LASER_TILT_SLOW, 00080 LASER_TILT_FAST 00081 }; 00082 00083 enum HeadSequence { 00084 HEAD_NOD, 00085 HEAD_SHAKE 00086 }; 00087 00088 public: 00089 00090 GeneralCommander(bool control_body, 00091 bool control_head, 00092 bool control_rarm, 00093 bool control_larm, 00094 bool control_prosilica, 00095 std::string arm_controller_name=default_arm_controller_name); 00096 00097 ~GeneralCommander(); 00098 00099 void setLaserMode(LaserControlMode mode); 00100 00101 void setHeadMode(HeadControlMode mode); 00102 00103 void setArmMode(WhichArm which, ArmControlMode mode); 00104 00105 LaserControlMode getLaserMode(); 00106 00107 HeadControlMode getHeadMode(); 00108 00109 //returns right if they are different and both are requested 00110 ArmControlMode getArmMode(WhichArm which); 00111 00112 void sendHeadCommand(double req_pan, double req_tilt); 00113 00114 void sendProjectorStartStop(bool start); 00115 00116 void sendGripperCommand(WhichArm which, bool close); 00117 00118 void sendHeadTrackCommand(); 00119 00120 void sendTorsoCommand(double pos, double vel); 00121 00122 void sendBaseCommand(double vx, double vy, double vw); 00123 00124 void switchControllers(const std::vector<std::string>& start_controllers, const std::vector<std::string>& stop_controllers); 00125 00126 void sendWristVelCommands(double right_wrist_vel, double left_wrist_vel, double hz); 00127 00128 bool getJointPosition(const std::string& name, double& pos) const; 00129 bool getJointVelocity(const std::string& name, double& vel) const; 00130 00131 void updateCurrentWristPositions(); 00132 00133 void sendArmVelCommands(double r_x_vel, double r_y_vel, double r_z_vel, double r_pitch_vel, double r_roll_vel, 00134 double l_x_vel, double l_y_vel, double l_z_vel, double l_pitch_vel, double l_roll_vel, 00135 double hz); 00136 00137 bool moveToWalkAlongArmPose(); 00138 00139 void sendWalkAlongCommand(double thresh, 00140 double x_dist_max, double x_speed_scale, 00141 double y_dist_max, double y_speed_scale, 00142 double rot_scale); 00143 00144 void sendHeadSequence(HeadSequence seq); 00145 00146 void requestProsilicaImage(std::string ns); 00147 00148 bool initWalkAlong(); 00149 00150 void tuckArms(WhichArm arm); 00151 void untuckArms(WhichArm arm); 00152 00153 bool isWalkAlongOk() { 00154 return walk_along_ok_; 00155 } 00156 00157 void turnOffWalkAlong() { 00158 walk_along_ok_ = false; 00159 } 00160 00161 private: 00162 00163 geometry_msgs::Pose getPositionFromJointsPose(ros::ServiceClient& service_client, 00164 std::string fk_link, 00165 const std::vector<std::string>& joint_names, const std::vector<double>& joint_pos); 00166 00167 void updateWalkAlongAverages(); 00168 00169 void jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState); 00170 00171 void powerBoardCallback(const pr2_msgs::PowerBoardStateConstPtr &powerBoardState); 00172 00173 void composeWristRotGoal(const std::string pref, pr2_controllers_msgs::JointTrajectoryGoal& goal, 00174 std::vector<double>& des_joints, double des_vel, double hz) const; 00175 00176 void clampDesiredArmPositionsToActual(double max_dist); 00177 00178 double calcAverage(const std::list<double>& av_list) const; 00179 00180 void unnormalizeTrajectory(trajectory_msgs::JointTrajectory& traj) const; 00181 00182 ros::NodeHandle n_; 00183 00184 bool control_body_; 00185 bool control_head_; 00186 bool control_rarm_; 00187 bool control_larm_; 00188 bool control_prosilica_; 00189 00190 double laser_slow_period_; 00191 double laser_slow_amplitude_; 00192 double laser_slow_offset_; 00193 00194 double laser_fast_period_; 00195 double laser_fast_amplitude_; 00196 double laser_fast_offset_; 00197 00198 std::string r_arm_controller_name_; 00199 std::string l_arm_controller_name_; 00200 00201 std::map<std::string, double> joint_state_position_map_; 00202 std::map<std::string, double> joint_state_velocity_map_; 00203 00204 geometry_msgs::Pose right_wrist_roll_pose_, left_wrist_roll_pose_; 00205 geometry_msgs::Pose des_r_wrist_roll_pose_, des_l_wrist_roll_pose_; 00206 00207 geometry_msgs::Pose walk_along_left_des_pose_, walk_along_right_des_pose_; 00208 std::vector<double> right_walk_along_pose_, left_walk_along_pose_; 00209 00210 std::vector<double> right_des_joint_states_, left_des_joint_states_; 00211 00212 std::list<double> walk_rdx_vals_, walk_rdy_vals_, walk_ldx_vals_, walk_ldy_vals_; 00213 bool walk_along_ok_; 00214 00215 trajectory_msgs::JointTrajectory head_nod_traj_, head_shake_traj_; 00216 00217 ros::ServiceClient tilt_laser_service_; 00218 ros::ServiceClient switch_controllers_service_; 00219 ros::ServiceClient right_arm_kinematics_solver_client_; 00220 ros::ServiceClient right_arm_kinematics_forward_client_; 00221 ros::ServiceClient right_arm_kinematics_inverse_client_; 00222 ros::ServiceClient left_arm_kinematics_solver_client_; 00223 ros::ServiceClient left_arm_kinematics_forward_client_; 00224 ros::ServiceClient left_arm_kinematics_inverse_client_; 00225 ros::ServiceClient prosilica_polling_client_; 00226 ros::Publisher head_pub_; 00227 ros::Publisher torso_pub_; 00228 ros::Publisher base_pub_; 00229 ros::Publisher right_arm_traj_pub_; 00230 ros::Publisher left_arm_traj_pub_; 00231 ros::Subscriber joint_state_sub_; 00232 ros::Subscriber power_board_sub_; 00233 00234 ros::Time last_right_wrist_goal_stamp_; 00235 ros::Time last_left_wrist_goal_stamp_; 00236 00237 double last_torso_vel_; 00238 00240 urdf::Model robot_model_; 00242 bool robot_model_initialized_; 00243 00244 bool status_projector_on; 00245 LaserControlMode laser_control_mode_; 00246 HeadControlMode head_control_mode_; 00247 ArmControlMode right_arm_control_mode_; 00248 ArmControlMode left_arm_control_mode_; 00249 00250 actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction>* head_track_hand_client_; 00251 actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>* right_gripper_client_; 00252 actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>* left_gripper_client_; 00253 actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* right_arm_trajectory_client_; 00254 actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* left_arm_trajectory_client_; 00255 actionlib::SimpleActionClient<pr2_common_action_msgs::TuckArmsAction>* tuck_arms_client_; 00256 };