cob_base_velocity_smoother.h
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2013
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering  
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_base_velocity_smoother
00013  *                                                      
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *              
00016  * Author: Florian Mirus, email:Florian.Mirus@ipa.fhg.de
00017  *
00018  * Date of creation: Jan 2013
00019  *
00020  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00021  *
00022  * Redistribution and use in source and binary forms, with or without
00023  * modification, are permitted provided that the following conditions are met:
00024  *
00025  *   * Redistributions of source code must retain the above copyright
00026  *       notice, this list of conditions and the following disclaimer.
00027  *   * Redistributions in binary form must reproduce the above copyright
00028  *       notice, this list of conditions and the following disclaimer in the
00029  *       documentation and/or other materials provided with the distribution.
00030  *   * Neither the name of the Fraunhofer Institute for Manufacturing 
00031  *       Engineering and Automation (IPA) nor the names of its
00032  *       contributors may be used to endorse or promote products derived from
00033  *       this software without specific prior written permission.
00034  *
00035  * This program is free software: you can redistribute it and/or modify
00036  * it under the terms of the GNU Lesser General Public License LGPL as 
00037  * published by the Free Software Foundation, either version 3 of the 
00038  * License, or (at your option) any later version.
00039  * 
00040  * This program is distributed in the hope that it will be useful,
00041  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00042  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00043  * GNU Lesser General Public License LGPL for more details.
00044  * 
00045  * You should have received a copy of the GNU Lesser General Public 
00046  * License LGPL along with this program. 
00047  * If not, see <http://www.gnu.org/licenses/>.
00048  *
00049  ****************************************************************/
00050 
00051 #ifndef COB_BASE_VELOCITY_SMOOTHER_H
00052 #define COB_BASE_VELOCITY_SMOOTHER_H
00053 
00054 //**************************** includes ************************/
00055 
00056 // standard includes
00057 #include <XmlRpc.h>
00058 #include <pthread.h>
00059 #include <deque>
00060 #include <sstream>
00061 #include <iostream>
00062 #include <boost/circular_buffer.hpp>
00063 #include <boost/bind.hpp>
00064 #include <pthread.h>
00065 
00066 using namespace std;
00067 
00068 // ros includes
00069 #include <ros/ros.h>
00070 #include <geometry_msgs/Twist.h>
00071 #include <ros/console.h>
00072 #include <std_msgs/String.h>
00073 /****************************************************************
00074  * the ros navigation doesn't run very smoothly because acceleration is too high
00075  * --> cob has strong base motors and therefore reacts with shaking behavior 
00076  * (PR2 has much more mechanical damping)
00077  * solution: the additional node cob_base_velocity_smoother smooths the velocities
00078  * comming from ROS-navigation or teleoperation, by calculating the mean values of a certain number 
00079  * of past messages and limiting the acceleration under a given threshold. 
00080  * cob_base_velocity_smoother subsribes (input) and publishes (output) geometry_msgs::Twist.
00081  ****************************************************************/
00082 class cob_base_velocity_smoother
00083 {
00084 private:
00085   //capacity for circular buffers (to be loaded from parameter server, otherwise set to default value 12)
00086   int buffer_capacity_;
00087   //maximal time-delay in seconds for stored messages in Circular Buffer (to be loaded from parameter server, otherwise set to default value 4)
00088   double store_delay_;
00089   // maximal time-delay in seconds to wait until filling the circular buffer with zero messages when the subscriber doesn't hear anything
00090   double stop_delay_after_no_sub_;
00091   //threshhold for maximal allowed acceleration (to be loaded from parameter server, otherwise set to default value 0.02)
00092   double acc_limit_;
00093   // ros loop rate (to be loaded from parameter server, otherwise set to default value 30)
00094   double loop_rate_;
00095   // delay between received commands that is allowed. After that, fill buffer with zeros.
00096   double max_delay_between_commands_;
00097   //geometry message filled with zero values
00098   geometry_msgs::Twist zero_values_;
00099   // subscribed geometry message
00100   geometry_msgs::Twist sub_msg_;
00101 
00102   pthread_mutex_t m_mutex;
00103   bool new_msg_received_;
00104 
00105   // function for checking wether a new msg has been received, triggering publishers accordingly
00106   void set_new_msg_received(bool received);
00107   bool get_new_msg_received();
00108 
00109 public:
00110 
00111   // constructor
00112   cob_base_velocity_smoother();
00113 
00114   // destructor
00115   ~cob_base_velocity_smoother();
00116 
00117   //create node handle
00118   ros::NodeHandle nh_, pnh_;
00119 
00120   //circular buffers for velocity, output and time
00121   boost::circular_buffer<geometry_msgs::Twist> cb_;
00122   boost::circular_buffer<geometry_msgs::Twist> cb_out_;
00123   boost::circular_buffer<ros::Time> cb_time_;
00124 
00125   // declaration of ros subscribers
00126   ros::Subscriber geometry_msgs_sub_;
00127 
00128   // decalaration of ros publishers
00129   ros::Publisher pub_;
00130 
00131   //callback function to subsribe to the geometry messages
00132   void geometryCallback(const geometry_msgs::Twist::ConstPtr &cmd_vel);
00133   //calculation function called periodically in main
00134   void calculationStep();
00135   //function that updates the circular buffer after receiving a new geometry message
00136   void reviseCircBuff(ros::Time now, geometry_msgs::Twist cmd_vel);
00137   //function to limit the acceleration under the given threshhold thresh
00138   void limitAcceleration(ros::Time now, geometry_msgs::Twist& cmd_vel);
00139 
00140   //boolean function that returns true if all messages stored in the circular buffer are older than store_delay, false otherwise
00141   bool circBuffOutOfDate(ros::Time now);
00142   // function to compare two geometry messages
00143   bool IsEqual(geometry_msgs::Twist msg1, geometry_msgs::Twist msg2);
00144 
00145   //boolean function that returns true if the input msg cmd_vel equals zero_values, false otherwise
00146   bool IsZeroMsg(geometry_msgs::Twist cmd_vel);
00147 
00148   //help-function that returns the signum of a double variable
00149   int signum(double var);
00150 
00151   //functions to calculate the mean values for each direction
00152   double meanValueX();
00153   double meanValueY();
00154   double meanValueZ();
00155 
00156   // function to make the loop rate available outside the class
00157   double getLoopRate();
00158 
00159   //function for the actual computation
00160   //calls the reviseCircBuff and the meanValue-functions and limits the acceleration under thresh
00161   //returns the resulting geometry message to be published to the base_controller
00162   geometry_msgs::Twist setOutput(ros::Time now, geometry_msgs::Twist cmd_vel);
00163 
00164 };
00165 
00166 #endif


cob_base_velocity_smoother
Author(s): Florian Mirus
autogenerated on Sun Oct 5 2014 23:06:22