joystick_control.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <sensor_msgs/Joy.h>
00032 #include <corobot_msgs/MoveArm.h>
00033 #include <corobot_msgs/MotorCommand.h>
00034 #include <corobot_msgs/takepic.h>
00035 #include <corobot_msgs/PanTilt.h>
00036 #include <std_msgs/Int32.h>
00037 #include <math.h>
00038 
00044 using namespace corobot_msgs;
00045 
00046 // Publishers that send commands to other packages after processing the joystick actions
00047 ros::Publisher driveControl_pub,takepic_pub,pan_tilt_control, move_arm_pub;
00048 
00049 // used to save some interesting valuesm such as speed, camera position... 
00050 int speed_left, speed_right, speed_value;
00051 bool turningLeft, turningRight;
00052 int pan_value,tilt_value;
00053 double ory, orz, orx;
00054 
00055 //0 = open, 1 = closed
00056 int gripper_state; 
00057 int save_image_state = 0;
00058 
00059 void velocityCallback(const std_msgs::Int32::ConstPtr& msg)
00060 {
00061         speed_value = msg->data;
00062 }
00063 
00064 // Receives the joystick actions and process them
00065 void joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
00066 {
00067 //********************************************
00068 //Motor control
00069 
00070         // the speed_left and speed_right parameters have to be between -100 and +100. The joy->axes value is between -1 and 1.
00071         speed_left = joy->axes[1] * 100;
00072         speed_right = joy->axes[1] * 100;
00073         speed_left += -joy->axes[0] * 100;
00074         speed_right += joy->axes[0] * 100;
00075 
00076         if(speed_right >100)
00077                 speed_right = 100;
00078         if(speed_right < -100)
00079                 speed_right = -100;
00080         if(speed_left >100)
00081                 speed_left = 100;
00082         if(speed_left < -100)
00083                 speed_left = -100;
00084 
00085     corobot_msgs::MotorCommand msg;
00086     msg.leftSpeed = speed_left;
00087     msg.rightSpeed = speed_right;
00088     // we want to drive for 3s only, so that if the connection is lost the robot will automatically stop after that time
00089     msg.secondsDuration = 3;
00090     msg.acceleration = 50;
00091     driveControl_pub.publish(msg);
00092  //stop
00093   if(joy->buttons[8]) 
00094   {
00095         corobot_msgs::MotorCommand msg;
00096         msg.leftSpeed = 0;
00097         msg.rightSpeed = 0;
00098         msg.secondsDuration = 0;
00099         msg.acceleration = 50;
00100         driveControl_pub.publish(msg);
00101   }
00102 //*********************************************************
00103 //Take picture
00104 // right Stick click Take Picture
00105  if(joy->buttons[11]) 
00106   {
00107     if(save_image_state == 0)
00108     {
00109         takepic msg;
00110         msg.camera_index = 1;
00111         msg.take = true;
00112         takepic_pub.publish(msg);
00113         save_image_state = 1;
00114     }
00115   }
00116   else
00117         save_image_state = 0;
00118 
00119 //********************************************************
00120 // Pan Tilt Control     
00121 // Pan control going to left
00122   if(joy->axes[2]>0.5) 
00123   {
00124         if(pan_value > -70)
00125         {
00126                 pan_value -= 5;
00127                 PanTilt msg;
00128                 msg.pan = pan_value;
00129                 msg.tilt = tilt_value;
00130                 msg.reset = 0;
00131                 pan_tilt_control.publish(msg);
00132         }
00133   }
00134 // Pan control going to right
00135   if(joy->axes[2]<-0.5) 
00136   {
00137         if(pan_value < 70)
00138         {
00139         pan_value += 5;
00140                 PanTilt msg;
00141                 msg.pan = pan_value;
00142                 msg.tilt = tilt_value;
00143                 msg.reset = 0;
00144                 pan_tilt_control.publish(msg);
00145         }
00146   }
00147 // Tilt control going down
00148   if(joy->axes[3]<-0.5) 
00149   {
00150         if(tilt_value > -30)
00151         {
00152         tilt_value -= 5;
00153                 PanTilt msg;
00154                 msg.pan = pan_value;
00155                 msg.tilt = tilt_value;
00156                 msg.reset = 0;
00157                 pan_tilt_control.publish(msg);
00158         }
00159   }
00160 // Tilt control going up
00161   if(joy->axes[3]>0.5) 
00162   {
00163         if(tilt_value < 30)
00164         {
00165         tilt_value += 5;
00166                 PanTilt msg;
00167                 msg.pan = pan_value;
00168                 msg.tilt = tilt_value;
00169                 msg.reset = 0;
00170                 pan_tilt_control.publish(msg);
00171         }
00172   }
00173 // reset the pan and tilt position
00174  if(joy->buttons[9]) 
00175   {
00176     tilt_value = 0;
00177     pan_value = 0;
00178     PanTilt msg;
00179     msg.pan = pan_value;
00180     msg.tilt = tilt_value;
00181     msg.reset = 0;
00182     pan_tilt_control.publish(msg);
00183   }
00184 
00185 //*****************************************************
00186 
00187 //*****************************************************
00188 //Arm control
00189 
00190 // Shoulder control
00191 if(joy->axes[4]>0.5) 
00192   {
00193         if(ory > 0.7)
00194         {
00195                 ory -= M_PI/8;
00196 
00197                 MoveArm msg1;
00198                 msg1.index = MoveArm::SHOULDER;
00199                 // The value has to be in degrees
00200                 msg1.position = ory * 180 / M_PI; 
00201 
00202                 move_arm_pub.publish(msg1);
00203         }
00204   }
00205 // Shoulder control
00206   if(joy->axes[4]<-0.5) 
00207   {
00208     
00209         if( ory < 2)
00210         {
00211                 ory += M_PI/8;
00212 
00213                 MoveArm msg1;
00214                 msg1.index = MoveArm::SHOULDER;
00215                 // The value has to be in degrees
00216                 msg1.position = ory * 180 / M_PI; 
00217 
00218                 move_arm_pub.publish(msg1);
00219         }
00220   }
00221 // Elbow control
00222   if(joy->axes[5]<-0.5) 
00223   {
00224     
00225     if( orz > 0.8)
00226     {
00227                 orz -= M_PI/8;
00228 
00229                 MoveArm msg1;
00230                 msg1.index = MoveArm::ELBOW;
00231                 // The value has to be in degrees
00232                 msg1.position = orz * 180 / M_PI; 
00233 
00234                 move_arm_pub.publish(msg1);
00235         }
00236   }
00237 // Elbow control
00238   if(joy->axes[5]>0.5) 
00239   {
00240     
00241     if( orz < 2.5)
00242     {
00243                 orz += M_PI/8;
00244 
00245                 MoveArm msg1;
00246                 msg1.index = MoveArm::ELBOW;
00247                 // The value has to be in degrees
00248                 msg1.position = orz * 180 / M_PI; 
00249 
00250                 move_arm_pub.publish(msg1);
00251         }
00252   }
00253 // arm reset
00254  if(joy->buttons[7]) 
00255   {
00256         ory = orz = M_PI/2;
00257 
00258         MoveArm msg1;
00259         msg1.index = MoveArm::ELBOW;
00260         // The value has to be in degrees
00261         msg1.position = orz * 180 / M_PI; 
00262 
00263         move_arm_pub.publish(msg1);
00264 
00265         msg1.index = MoveArm::SHOULDER;
00266         // The value has to be in degrees
00267         msg1.position = ory * 180 / M_PI; 
00268 
00269         move_arm_pub.publish(msg1);
00270   }
00271 //******************************************
00272 //wrist control
00273 
00274 // Wrist Left
00275  if(joy->buttons[4]) 
00276   {
00277     
00278     if(orx < 4.5)
00279     {
00280           orx += 0.5;
00281 
00282           MoveArm msg1;
00283           msg1.index = MoveArm::WRIST_ROTATION;
00284           // The value has to be in degrees
00285           msg1.position = orx * 180 / M_PI; 
00286 
00287           move_arm_pub.publish(msg1);
00288         }
00289   }
00290 // Wrist Right
00291  if(joy->buttons[5]) 
00292   {
00293     if(orx > 0.1)
00294     {
00295           orx -= 0.5;
00296 
00297           MoveArm msg1;
00298           msg1.index = MoveArm::WRIST_ROTATION;
00299           // The value has to be in degrees
00300           msg1.position = orx * 180 / M_PI; 
00301           move_arm_pub.publish(msg1);
00302         }
00303   }
00304 //****************************************************
00305 //gripper control
00306   if(joy->buttons[6])
00307   {
00308 
00309     if(gripper_state == 0)
00310         gripper_state = 1;
00311     else if(gripper_state == 1)
00312         gripper_state = 0;
00313 
00314     MoveArm msg1;
00315     msg1.index = MoveArm::GRIPPER;
00316     // The value has to be in degrees
00317     msg1.position = (gripper_state * 180); 
00318 
00319     move_arm_pub.publish(msg1);
00320 
00321   }
00322 
00323 }
00324 
00325 
00326 int main(int argc, char** argv)
00327 {
00328   
00329   pan_value = 0;
00330   tilt_value = 0;
00331   //initial value for wrist
00332   orx = 2.3;
00333   ory = M_PI/2;
00334   orz = M_PI/2;
00335   gripper_state = 0;
00336   speed_left = 0;
00337   speed_right = 0;
00338   speed_value = 75;
00339   turningLeft = false;
00340   turningRight = false;
00341 
00342   ros::init(argc, argv, "corobot_joystick");
00343   
00344   ros::NodeHandle n;
00345 
00346 // Advertize topics used to control the robot
00347   driveControl_pub = n.advertise<corobot_msgs::MotorCommand>("PhidgetMotor", 100);
00348   takepic_pub = n.advertise<takepic>("takepicture",100);
00349   pan_tilt_control = n.advertise<PanTilt>("pantilt",10);
00350   move_arm_pub = n.advertise<MoveArm>("armPosition",10);
00351 
00352   ros::Subscriber sub = n.subscribe<sensor_msgs::Joy>("joy", 1000, joyCallback);
00353   //choose the maximum speed for the robot. value is between 0 and 100, 100 beeing the fastest possible
00354   ros::Subscriber velocity = n.subscribe<std_msgs::Int32>("velocityValue", 1000, velocityCallback);
00355   
00356   ros::spin();
00357 }


corobot_joystick
Author(s): Gang Li/GangLi@coroware.com
autogenerated on Wed Aug 26 2015 11:09:57