uos_diffdrive_teleop_ps3joy.cpp
Go to the documentation of this file.
00001 /*
00002  *
00003  * Copyright (C) 2015 University of Osnabrück, Germany
00004  * 
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * uos_diffdrive_teleop_ps3.cpp
00020  *
00021  *  Created on: 16.02.2015
00022  *      Author: Sebastian Pütz <spuetz@uos.de>
00023  */
00024 
00025 #include <uos_diffdrive_teleop_ps3joy.h>
00026 
00027 TeleopPS3::TeleopPS3(){
00028   ros::NodeHandle n_private("~");
00029   //enable button pressure intensity
00030   n_private.param("use_button_pressure", use_button_pressure, false);
00031   joy_sub = n_.subscribe<sensor_msgs::Joy>("joy", 15,  &TeleopPS3::PS3Callback, this);
00032 }
00033 
00034 void TeleopPS3::PS3Callback(const sensor_msgs::Joy::ConstPtr& joy) {
00035 
00036   // factor elem of [0 1]
00037   double factor_x = 0;
00038   double factor_y = 0;
00039   double fac_button_x = 0;
00040   double fac_button_y = 0;
00041 
00042   // read the stick intensity factor
00043   double fac_stick_y = joy->axes[PS3_AXIS_STICK_LEFT_UPWARDS];
00044   double fac_stick_x = joy->axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];
00045 
00046   // if required, read the button pressure as intensity factor
00047   if(use_button_pressure) {
00048     // left, normalize scale  
00049     double fac_button_up = (1-joy->axes[PS3_AXIS_BUTTON_CROSS_UP])/2.0;
00050     double fac_button_down = -(1-joy->axes[PS3_AXIS_BUTTON_CROSS_DOWN])/2.0;
00051     fac_button_y = fac_button_up + fac_button_down;
00052     // right, normalize scale
00053     double fac_button_left = (1-joy->axes[PS3_AXIS_BUTTON_CROSS_LEFT])/2.0;
00054     double fac_button_right = -(1-joy->axes[PS3_AXIS_BUTTON_CROSS_RIGHT])/2.0;
00055     fac_button_x = fac_button_left + fac_button_right;
00056   } else { 
00057     // no button pressure required, check if the factor should be 1 or 0
00058     // if there is no button pressure enabled the factor should be 1
00059     fac_button_y = joy->buttons[PS3_BUTTON_CROSS_UP] - joy->buttons[PS3_BUTTON_CROSS_DOWN];
00060     fac_button_x = joy->buttons[PS3_BUTTON_CROSS_LEFT] - joy->buttons[PS3_BUTTON_CROSS_RIGHT]; 
00061   }
00062   // enable both, stick and buttons at the same time
00063   factor_x = fac_button_x + fac_stick_x; // can be > 1 or < -1
00064   factor_y = fac_button_y + fac_stick_y; // can be > 1 or < -1
00065 
00066   in.forwards = factor_y;
00067   in.left = factor_x;
00068   in.updated = true;
00069 }
00070 
00071 int main(int argc, char** argv)
00072 {
00073   ros::init(argc, argv, "uos_diffdrive_teleop_ps3");
00074   TeleopPS3 teleop;
00075   ros::spin();
00076   return EXIT_SUCCESS;
00077 }


uos_diffdrive_teleop
Author(s): Jochen Sprickerhof, Sebastian Pütz
autogenerated on Wed May 24 2017 03:03:01