uos_diffdrive_teleop.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.cpp
00020  *
00021  *  Created on: 16.02.2015
00022  *      Author: Sebastian Pütz <spuetz@uos.de>
00023  */
00024 
00025 #include <uos_diffdrive_teleop.h>
00026 
00027 Teleop::Teleop(){
00028   ros::NodeHandle n_private("~");
00029 
00030   n_private.param("max_vel", max_vel, 1.0);           // max velocity
00031   n_private.param("max_rot_vel", max_rot_vel, 2.0);   // max rot velocity
00032 
00033   // acc_ahead_* resp. acc_y.* describes the forwards/backwards part
00034   // acc_rot_* resp. acc_x.* describes the rotational part
00035 
00036   // *_pos describes the positive acceleration
00037 
00038   // *_neg describes the negative acceleration
00039   //    and is used to decelerate, while not accelerating
00040 
00041   // *_stop describes the active deceleration and 
00042   //    works as a break and it is used when pressing the opposite
00043   //    direction button
00044 
00045   //forward acceleration parameters
00046   n_private.param("acc_ahead_pos", acc_y.pos, 0.4);
00047   n_private.param("acc_ahead_neg", acc_y.neg, 0.8);
00048   n_private.param("acc_ahead_stop", acc_y.stop, 2.0);
00049 
00050   //rotational acceleration parameters
00051   n_private.param("acc_rot_pos", acc_x.pos, 1.4);
00052   n_private.param("acc_rot_neg", acc_x.neg, 1.8);
00053   n_private.param("acc_rot_stop", acc_x.stop, 2.6);
00054 
00055   //update rates
00056   n_private.param("update_velocity_rate", update_velocity_rate, 0.01);
00057   n_private.param("update_inputs_rate", update_inputs_rate, 0.05);
00058   
00059   vel_pub = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00060 
00061   vel_timer = n_.createTimer(ros::Duration(update_velocity_rate), &Teleop::updateVelocity, this);
00062   key_timer = n_.createTimer(ros::Duration(update_inputs_rate),  &Teleop::updateInputs, this);
00063 
00064   velo.x = 0;
00065   velo.y = 0;
00066 }
00067 
00068 void Teleop::updateVelocity(const ros::TimerEvent &t_event){
00069   ros::Duration delta = t_event.current_real - t_event.last_real;
00070   
00071   double left = in.left;
00072   double forwards = in.forwards;
00073 
00074   left = std::min(1.0, left);
00075   left = std::max(-1.0, left);
00076   forwards = std::min(1.0, forwards);
00077   forwards = std::max(-1.0, forwards);
00078 
00079   velo.y = adaptVelocity(delta.toSec(), velo.y, forwards, acc_y.stop, acc_y.neg, acc_y.pos);
00080   velo.x = adaptVelocity(delta.toSec(), velo.x, left, acc_x.stop, acc_x.neg, acc_x.pos);
00081 
00082   // velocity limits by intensity
00083   velo.dyn_limit_y = max_vel * forwards;
00084   velo.dyn_limit_x = max_rot_vel * left;
00085 
00086   // observe the limits for the forward part
00087   if((forwards < 0 && velo.y < velo.dyn_limit_y)
00088       || (forwards > 0 && velo.y > velo.dyn_limit_y))
00089     velo.y = velo.dyn_limit_y;
00090 
00091   // observe the limits for the rotational part
00092   if((left < 0 && velo.x < velo.dyn_limit_x)
00093       || (left > 0 && velo.x > velo.dyn_limit_x))
00094     velo.x = velo.dyn_limit_x;
00095 
00096   // set command 
00097   vel_cmd.linear.x = velo.y;
00098   vel_cmd.angular.z = velo.x;
00099 
00100   //publish command
00101   if(std::abs(vel_cmd.linear.x) > EPSILON_VELO || std::abs(vel_cmd.angular.z) > EPSILON_VELO) {
00102     vel_pub.publish(vel_cmd);
00103   }
00104   // TODO apply velocity by service call
00105 }
00106 
00107 void Teleop::updateInputs(const ros::TimerEvent &t_event){
00108   if(!in.updated){
00109     in.forwards = 0;
00110     in.left = 0;
00111   }
00112   in.updated = false;
00113 }
00114 
00115 double Teleop::adaptVelocity(
00116     double delta_time, // delta time
00117     double velocity,  // current velocity
00118     double factor,    // intensity factor
00119     double acc_stop,  // active deceleration 
00120     double acc_neg,   // deceleartion, if no button is pressed
00121     double acc_pos)   // positive acceleration
00122 {
00123 
00124   double new_velocity = velocity;
00125 
00126   if(factor != 0) {
00127     if(factor > 0) {
00128       // active decelerate
00129       if(velocity < 0)
00130         return (new_velocity += acc_stop * factor * delta_time) > -EPSILON_VELO ? 0 : new_velocity;
00131       // accelerate
00132       else 
00133         return new_velocity + acc_pos * factor * delta_time;
00134     } else {
00135       // active decelerate
00136       if(velocity > 0)
00137         return (new_velocity += acc_stop * factor * delta_time) < EPSILON_VELO ? 0 : new_velocity;
00138       // accelerate
00139       else
00140         return new_velocity + acc_pos * factor * delta_time;
00141     }
00142   }
00143   else {
00144     // nothing pressed -> decelerate
00145     if(velocity > 0)
00146       return (new_velocity += -acc_neg * delta_time) < EPSILON_VELO ? 0 : new_velocity;
00147     else if(velocity < 0)
00148       return (new_velocity += acc_neg * delta_time) > -EPSILON_VELO ? 0 : new_velocity;
00149     return 0;
00150   }
00151 }


uos_diffdrive_teleop
Author(s): Jochen Sprickerhof, Sebastian Pütz
autogenerated on Sat Jun 8 2019 19:40:48