pr2_teleop_general_commander.h
Go to the documentation of this file.
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_roll_vel, double r_pitch_vel, double r_yaw_vel,
00134                           double l_x_vel, double l_y_vel, double l_z_vel, double l_roll_vel, double l_pitch_vel, double l_yaw_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 };


pr2_teleop_general
Author(s): Gil Jones
autogenerated on Fri Aug 28 2015 12:05:50