uos_diffdrive_teleop.h
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.h
00020  *
00021  *  Created on: 16.02.2015
00022  *      Author: Sebastian Pütz <spuetz@uos.de>
00023  */
00024 #ifndef UOS_DIFFDRIVE_TELEOP_H
00025 #define UOS_DIFFDRIVE_TELEOP_H
00026 
00027 #include <ros/ros.h>
00028 #include <geometry_msgs/Twist.h>
00029 #include <algorithm>
00030 
00031 #define EPSILON_VELO 1e-3
00032 
00033 class Teleop
00034 {
00035   private:
00036     ros::Publisher vel_pub;
00037     ros::Timer vel_timer;
00038     ros::Timer key_timer;
00039   
00040     double update_inputs_rate;
00041     double update_velocity_rate;
00042 
00043     void updateVelocity(const ros::TimerEvent &t_event);
00044     void updateInputs(const ros::TimerEvent &t_event);
00045 
00046     double max_vel;
00047     double max_rot_vel;
00048     
00049     geometry_msgs::Twist vel_cmd;
00050 
00051     struct acceleration{
00052       double pos;
00053       double neg;
00054       double stop;
00055     };
00056     acceleration acc_y;
00057     acceleration acc_x;
00058 
00059     struct velocity{
00060       double x;
00061       double y;
00062       double dyn_limit_x;
00063       double dyn_limit_y;
00064     };
00065     velocity velo;
00066 
00067     
00068     // inputs elem of [0 1]
00069     struct inputs{
00070       bool updated;
00071       double forwards;
00072       double left;
00073     };
00074 
00075     
00076     double adaptVelocity( 
00077         double time_delta,
00078         double velocity,
00079         double factor,
00080         double acc_stop,
00081         double acc_neg,
00082         double acc_pos);
00083 
00084   protected:
00085     inputs in;
00086     ros::NodeHandle n_;
00087 
00088   public:
00089     Teleop();
00090 };
00091 
00092 #endif /* uos_diffdrive_teleop.h */


uos_diffdrive_teleop
Author(s): Jochen Sprickerhof, Sebastian Pütz
autogenerated on Wed Aug 26 2015 16:40:15