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 #include <ros/ros.h>
00035 #include <sensor_msgs/Joy.h>
00036 #include <geometry_msgs/Twist.h>
00037 #include <robotnik_msgs/set_mode.h>
00038 #include <robotnik_msgs/ptz.h>
00039 #include <guardian_joystick/enable_disable.h>
00040
00041
00042 #define DEFAULT_NUM_OF_BUTTONS 16
00043 #define DEFAULT_AXIS_LINEAR_X 1
00044 #define DEFAULT_AXIS_LINEAR_Y 0
00045 #define DEFAULT_AXIS_LINEAR_Z 3
00046 #define DEFAULT_AXIS_ANGULAR 2
00047 #define DEFAULT_SCALE_LINEAR_X 1.0
00048 #define DEFAULT_SCALE_LINEAR_Y 1.0
00049 #define DEFAULT_SCALE_LINEAR_Z 1.0
00050 #define DEFAULT_SCALE_ANGULAR 2.0
00051 #define NUM_BUTTONS 20
00052
00053 class GuardianJoy
00054 {
00055 public:
00056 GuardianJoy();
00057 bool EnableDisable(guardian_joystick::enable_disable::Request &req, guardian_joystick::enable_disable::Response &res );
00058
00059 private:
00060 void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
00061
00062 ros::NodeHandle nh_;
00063
00064 int linear_x_, linear_y_, linear_z_, angular_;
00065 double l_scale_x_, l_scale_y_, l_scale_z_, a_scale_;
00067 ros::Publisher vel_pub_;
00068 ros::Publisher ptz_pub_;
00070 ros::Subscriber joy_sub_;
00072 std::string cmd_topic_vel_;
00074 std::string cmd_service_io_;
00076 std::string cmd_topic_ptz_;
00077 double current_vel;
00079 int dead_man_button_;
00081 int speed_up_button_, speed_down_button_;
00082 int button_output_1_, button_output_2_;
00083 int output_1_, output_2_;
00084 bool bOutput1, bOutput2;
00086 ros::ServiceServer enable_disable_srv_;
00088 int button_kinematic_mode_;
00090 int kinematic_mode_;
00092 ros::ServiceClient setKinematicMode;
00094 std::string cmd_set_mode_;
00096 int ptz_tilt_up_, ptz_tilt_down_, ptz_pan_right_, ptz_pan_left_;
00098 int pan_increment_, tilt_increment_;
00100 ros::ServiceClient modbus_write_do_client;
00102 int num_of_buttons_;
00104 bool bRegisteredButtonEvent[NUM_BUTTONS];
00106 bool bEnable;
00107
00108 };
00109
00110 GuardianJoy::GuardianJoy():
00111 linear_x_(1),
00112 linear_y_(0),
00113 angular_(2),
00114 linear_z_(3)
00115 {
00116
00117 current_vel = 0.1;
00118
00119 nh_.param("num_of_buttons", num_of_buttons_, DEFAULT_NUM_OF_BUTTONS);
00120
00121 nh_.param("axis_linear_x", linear_x_, DEFAULT_AXIS_LINEAR_X);
00122 nh_.param("axis_linear_y", linear_y_, DEFAULT_AXIS_LINEAR_Y);
00123 nh_.param("axis_linear_z", linear_z_, DEFAULT_AXIS_LINEAR_Z);
00124 nh_.param("axis_angular", angular_, DEFAULT_AXIS_ANGULAR);
00125 nh_.param("scale_angular", a_scale_, DEFAULT_SCALE_ANGULAR);
00126 nh_.param("scale_linear_x", l_scale_x_, DEFAULT_SCALE_LINEAR_X);
00127 nh_.param("scale_linear_y", l_scale_y_, DEFAULT_SCALE_LINEAR_Y);
00128 nh_.param("scale_linear_z", l_scale_z_, DEFAULT_SCALE_LINEAR_Z);
00129 nh_.param("cmd_topic_vel", cmd_topic_vel_, cmd_topic_vel_);
00130 nh_.param("button_dead_man", dead_man_button_, dead_man_button_);
00131 nh_.param("button_speed_up", speed_up_button_, speed_up_button_);
00132 nh_.param("button_speed_down", speed_down_button_, speed_down_button_);
00133
00134
00135 nh_.param("cmd_service_io", cmd_service_io_, cmd_service_io_);
00136 nh_.param("button_output_1", button_output_1_, button_output_1_);
00137 nh_.param("button_output_2", button_output_2_, button_output_2_);
00138 nh_.param("output_1", output_1_, output_1_);
00139 nh_.param("output_2", output_2_, output_2_);
00140
00141 nh_.param("cmd_topic_ptz", cmd_topic_ptz_, cmd_topic_ptz_);
00142 nh_.param("button_ptz_tilt_up", ptz_tilt_up_, ptz_tilt_up_);
00143 nh_.param("button_ptz_tilt_down", ptz_tilt_down_, ptz_tilt_down_);
00144 nh_.param("button_ptz_pan_right", ptz_pan_right_, ptz_pan_right_);
00145 nh_.param("button_ptz_pan_left", ptz_pan_left_, ptz_pan_left_);
00146 nh_.param("pan_increment", pan_increment_, 1);
00147 nh_.param("tilt_increment",tilt_increment_, 1);
00148
00149
00150 nh_.param("button_kinematic_mode", button_kinematic_mode_, button_kinematic_mode_);
00151 nh_.param("cmd_service_set_mode", cmd_set_mode_, cmd_set_mode_);
00152 kinematic_mode_ = 1;
00153
00154
00155 for(int i = 0; i < NUM_BUTTONS; i++){
00156 bRegisteredButtonEvent[i] = false;
00157 }
00158
00159 ROS_INFO("Topic PTZ = [%s]", cmd_topic_ptz_.c_str());
00160 ROS_INFO("Service set_mode = [%s]", cmd_set_mode_.c_str());
00161 ROS_INFO("Service I/O = [%s]", cmd_topic_vel_.c_str());
00162 ROS_INFO("Axis linear X = %d", linear_x_);
00163 ROS_INFO("Axis linear Y = %d", linear_y_);
00164 ROS_INFO("Axis linear Z = %d", linear_z_);
00165 ROS_INFO("Axis angular = %d", angular_);
00166 ROS_INFO("Scale angular = %f", a_scale_);
00167 ROS_INFO("Deadman button = %d", dead_man_button_);
00168 ROS_INFO("OUTPUT1 button %d", button_output_1_);
00169 ROS_INFO("OUTPUT2 button %d", button_output_2_);
00170 ROS_INFO("OUTPUT1 button %d", button_output_1_);
00171 ROS_INFO("OUTPUT2 button %d", button_output_2_);
00172 ROS_INFO("Kinematic mode button %d", button_kinematic_mode_);
00173
00174
00175
00176 vel_pub_ = nh_.advertise<geometry_msgs::Twist>(cmd_topic_vel_, 1);
00177
00178
00179 ptz_pub_ = nh_.advertise<robotnik_msgs::ptz>(cmd_topic_ptz_, 1);
00180
00181
00182 joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &GuardianJoy::joyCallback, this);
00183
00184
00185
00186 bOutput1 = bOutput2 = false;
00187
00188
00189 setKinematicMode = nh_.serviceClient<robotnik_msgs::set_mode>(cmd_set_mode_);
00190
00191
00192 enable_disable_srv_ = nh_.advertiseService("/guardian_joystick/enable_disable", &GuardianJoy::EnableDisable, this);
00193
00194 bEnable = true;
00195
00196 }
00197
00198
00199
00200
00201
00202 bool GuardianJoy::EnableDisable(guardian_joystick::enable_disable::Request &req, guardian_joystick::enable_disable::Response &res )
00203 {
00204 bEnable = req.value;
00205
00206 ROS_INFO("GuardianJoystick::EnablaDisable: Setting to %d", req.value);
00207 res.ret = true;
00208 return true;
00209 }
00210
00211 void GuardianJoy::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
00212 {
00213 geometry_msgs::Twist vel;
00214 robotnik_msgs::ptz ptz;
00215 bool ptzEvent = false;
00216
00217
00218 if (joy->buttons[dead_man_button_] == 1) {
00219
00220
00221 if ( joy->buttons[speed_down_button_] == 1 ){
00222
00223 if(!bRegisteredButtonEvent[speed_down_button_])
00224 if(current_vel > 0.1){
00225 current_vel = current_vel - 0.1;
00226 bRegisteredButtonEvent[speed_down_button_] = true;
00227 ROS_INFO("Velocity: %f%%", current_vel*100.0);
00228 }
00229 }else{
00230 bRegisteredButtonEvent[speed_down_button_] = false;
00231 }
00232
00233 if (joy->buttons[speed_up_button_] == 1){
00234 if(!bRegisteredButtonEvent[speed_up_button_])
00235 if(current_vel < 0.9){
00236 current_vel = current_vel + 0.1;
00237 bRegisteredButtonEvent[speed_up_button_] = true;
00238 ROS_INFO("Velocity: %f%%", current_vel*100.0);
00239 }
00240
00241 }else{
00242 bRegisteredButtonEvent[speed_up_button_] = false;
00243 }
00244
00245 vel.angular.x = current_vel*(a_scale_*joy->axes[angular_]);
00246 vel.angular.y = current_vel*(a_scale_*joy->axes[angular_]);
00247 vel.angular.z = current_vel*(a_scale_*joy->axes[angular_]);
00248 vel.linear.x = current_vel*l_scale_x_*joy->axes[linear_x_];
00249 vel.linear.y = current_vel*l_scale_y_*joy->axes[linear_y_];
00250 vel.linear.z = current_vel*l_scale_z_*joy->axes[linear_z_];
00251
00252
00253 if (joy->buttons[button_output_1_] == 1) {
00254
00255 if(!bRegisteredButtonEvent[button_output_1_]){
00256
00257
00258
00259 bOutput1=!bOutput1;
00260
00261
00262 bRegisteredButtonEvent[button_output_1_] = true;
00263 }
00264 }else{
00265 bRegisteredButtonEvent[button_output_1_] = false;
00266 }
00267
00268 if (joy->buttons[button_output_2_] == 1) {
00269
00270 if(!bRegisteredButtonEvent[button_output_2_]){
00271
00272
00273
00274 bOutput2=!bOutput2;
00275
00276
00277 bRegisteredButtonEvent[button_output_2_] = true;
00278 }
00279 }else{
00280 bRegisteredButtonEvent[button_output_2_] = false;
00281 }
00282
00283
00284
00285
00286 ptz.pan = ptz.tilt = ptz.zoom = 0.0;
00287 ptz.relative = true;
00288 if (joy->buttons[ptz_tilt_up_] == 1) {
00289 if(!bRegisteredButtonEvent[ptz_tilt_up_]){
00290 ptz.tilt = tilt_increment_;
00291
00292 bRegisteredButtonEvent[ptz_tilt_up_] = true;
00293 ptzEvent = true;
00294 }
00295 }else {
00296 bRegisteredButtonEvent[ptz_tilt_up_] = false;
00297 }
00298
00299 if (joy->buttons[ptz_tilt_down_] == 1) {
00300 if(!bRegisteredButtonEvent[ptz_tilt_down_]){
00301 ptz.tilt = -tilt_increment_;
00302
00303 bRegisteredButtonEvent[ptz_tilt_down_] = true;
00304 ptzEvent = true;
00305 }
00306 }else{
00307 bRegisteredButtonEvent[ptz_tilt_down_] = false;
00308 }
00309
00310
00311 if (joy->buttons[ptz_pan_left_] == 1) {
00312 if(!bRegisteredButtonEvent[ptz_pan_left_]){
00313 ptz.pan = -pan_increment_;
00314
00315 bRegisteredButtonEvent[ptz_pan_left_] = true;
00316 ptzEvent = true;
00317 }
00318 }else{
00319 bRegisteredButtonEvent[ptz_pan_left_] = false;
00320 }
00321
00322 if (joy->buttons[ptz_pan_right_] == 1) {
00323 if(!bRegisteredButtonEvent[ptz_pan_right_]){
00324 ptz.pan = pan_increment_;
00325
00326 bRegisteredButtonEvent[ptz_pan_right_] = true;
00327 ptzEvent = true;
00328 }
00329 }else{
00330 bRegisteredButtonEvent[ptz_pan_right_] = false;
00331 }
00332
00333
00334 if (joy->buttons[button_kinematic_mode_] == 1) {
00335
00336 if(!bRegisteredButtonEvent[button_kinematic_mode_]){
00337
00338 kinematic_mode_ += 1;
00339 if (kinematic_mode_ > 2) kinematic_mode_ = 1;
00340 ROS_INFO("GuardianJoy::joyCallback: Kinematic Mode %d ", kinematic_mode_);
00341
00342 robotnik_msgs::set_mode set_mode_srv;
00343 set_mode_srv.request.mode = kinematic_mode_;
00344 setKinematicMode.call( set_mode_srv );
00345 bRegisteredButtonEvent[button_kinematic_mode_] = true;
00346 }
00347 }else{
00348 bRegisteredButtonEvent[button_kinematic_mode_] = false;
00349 }
00350
00351 }
00352 else {
00353 vel.angular.x = 0.0; vel.angular.y = 0.0; vel.angular.z = 0.0;
00354 vel.linear.x = 0.0; vel.linear.y = 0.0; vel.linear.z = 0.0;
00355 }
00356
00357 if (ptzEvent) ptz_pub_.publish(ptz);
00358
00359 vel_pub_.publish(vel);
00360 }
00361
00362
00363 int main(int argc, char** argv)
00364 {
00365 ros::init(argc, argv, "guardian_joystick");
00366 GuardianJoy Guardian_joy;
00367 ros::spin();
00368 }
00369