Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "ros/ros.h"
00036 #include "geometry_msgs/Twist.h"
00037 #include "sensor_msgs/Joy.h"
00038
00039 double MAX_SPEED_MULT = .5;
00040 double MAX_TURN_MULT = .25;
00041
00042 bool firstZeroVel = false;
00043 bool publishAnyway = false;
00044
00045
00046 int lastButtons[11];
00047 float lastAxes[8];
00048
00049
00050 void joystickCallback(const sensor_msgs::Joy::ConstPtr& msg)
00051 {
00052
00053
00054 int x;
00055 for(x = 0; x < 11; x++)
00056 lastButtons[x] = msg->buttons[x];
00057
00058 int y;
00059 for(y = 0; y < 8; y++)
00060 lastAxes[y] = msg->axes[y];
00061
00062 if(lastAxes[1] < 0.2 && lastAxes[1] > -0.2)
00063 lastAxes[1] = 0;
00064
00065 if(lastAxes[3] < 0.2 && lastAxes[3] > -0.2)
00066 lastAxes[3] = 0;
00067
00068
00069 if(lastAxes[1] == 0 && lastAxes[3] == 0 && lastAxes[6] == 0 && lastAxes[7] == 0)
00070 firstZeroVel = true;
00071
00072
00073 if(lastAxes[6] == -1)
00074 {
00075 MAX_TURN_MULT += .05;
00076 ROS_INFO("Turn Multiplier Changed: %f -> %f\n", MAX_TURN_MULT - .05, MAX_TURN_MULT);
00077 }
00078
00079
00080 if(lastAxes[6] == 1)
00081 {
00082 MAX_TURN_MULT -= .05;
00083 ROS_INFO("Turn Multiplier Changed: %f -> %f\n", MAX_TURN_MULT + .05, MAX_TURN_MULT);
00084 }
00085
00086
00087 if(lastAxes[7] == -1)
00088 {
00089 MAX_SPEED_MULT -= .1;
00090 ROS_INFO("Speed Multiplier Changed: %f -> %f\n", MAX_SPEED_MULT + .1, MAX_SPEED_MULT);
00091 }
00092
00093
00094 if(lastAxes[7] == 1)
00095 {
00096 MAX_SPEED_MULT += .1;
00097 ROS_INFO("Speed Multiplier Changed: %f -> %f\n", MAX_SPEED_MULT - .1, MAX_SPEED_MULT);
00098 }
00099
00100
00101
00102 if(lastButtons[1] == 1)
00103 {
00104 lastAxes[1] = 0;
00105 lastAxes[3] = 0;
00106 publishAnyway = true;
00107 }
00108 else
00109 {
00110 publishAnyway = false;
00111 }
00112 }
00113
00114 int main(int argc, char **argv)
00115 {
00116
00117 if(!(argc == 1 || argc == 3))
00118 ROS_INFO("Invalid number of starting parameters, ignoring all of them.");
00119
00120
00121 if(argc == 3)
00122 {
00123 MAX_SPEED_MULT = atof(argv[1]);
00124 MAX_TURN_MULT = atof(argv[2]);
00125 ROS_INFO("Starting with Speed Mult of %f, and Turn Mult of %f", MAX_SPEED_MULT, MAX_TURN_MULT);
00126 }
00127
00128
00129 if(MAX_SPEED_MULT == 0)
00130 {
00131 MAX_SPEED_MULT = .5;
00132 ROS_INFO("Adjusted Speed Mult to .5");
00133 }
00134
00135
00136 if(MAX_TURN_MULT == 0)
00137 {
00138 MAX_TURN_MULT = .25;
00139 ROS_INFO("Adjusted Turn Mult to .25");
00140 }
00141
00142 ros::init(argc, argv, "joystick_teleop");
00143
00144 ros::NodeHandle n;
00145
00146 ros::Publisher pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
00147
00148 ros::Subscriber sub = n.subscribe("joy", 1000, joystickCallback);
00149
00150 ros::Rate loop_rate(10);
00151
00152 while (ros::ok())
00153 {
00154 geometry_msgs::Twist msg;
00155
00156
00157 msg.linear.x = lastAxes[1] * MAX_SPEED_MULT;
00158
00159
00160 msg.angular.z = lastAxes[3] * MAX_TURN_MULT;
00161
00162
00163 if(msg.linear.x == 0 && msg.angular.z == 0)
00164 {
00165 if(firstZeroVel == true || publishAnyway == true)
00166 {
00167 pub.publish(msg);
00168 firstZeroVel = false;
00169 }
00170 }
00171 else
00172 {
00173 pub.publish(msg);
00174 }
00175
00176 ros::spinOnce();
00177
00178 loop_rate.sleep();
00179 }
00180
00181
00182 return 0;
00183 }