cob_base_velocity_smoother.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2012
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: May 2012
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 #include <cob_base_velocity_smoother.h>
00052 
00053 /****************************************************************
00054  * the ros navigation doesn't run very smoothly because acceleration is too high
00055  * --> cob has strong base motors and therefore reacts with shaking behavior 
00056  * (PR2 has much more mechanical damping)
00057  * solution: the additional node cob_base_velocity_smoother smooths the velocities
00058  * comming from ROS-navigation or teleoperation, by calculating the mean values of a certain number 
00059  * of past messages and limiting the acceleration under a given threshold. 
00060  * cob_base_velocity_smoother subsribes (input) and publishes (output) geometry_msgs::Twist.
00061  ****************************************************************/
00062 
00063 // function for checking wether a new msg has been received, triggering publishers accordingly
00064 void cob_base_velocity_smoother::set_new_msg_received(bool received)
00065 {
00066   pthread_mutex_lock(&m_mutex);
00067   new_msg_received_ = received;
00068   pthread_mutex_unlock(&m_mutex);
00069 }
00070 
00071 bool cob_base_velocity_smoother::get_new_msg_received()
00072 {
00073   pthread_mutex_lock(&m_mutex);
00074   bool ret_val = new_msg_received_;
00075   pthread_mutex_unlock(&m_mutex);
00076   return ret_val;
00077 }
00078 
00079 // constructor
00080 cob_base_velocity_smoother::cob_base_velocity_smoother()
00081 {
00082 
00083   // 
00084   m_mutex = PTHREAD_MUTEX_INITIALIZER;
00085   new_msg_received_ = false;
00086 
00087   // create node handles
00088   nh_ = ros::NodeHandle();
00089   pnh_ = ros::NodeHandle("~");
00090 
00091   // publisher
00092   pub_ = nh_.advertise<geometry_msgs::Twist>("output", 1);
00093 
00094   // subscriber
00095   geometry_msgs_sub_ = nh_.subscribe<geometry_msgs::Twist>("input", 1, boost::bind(&cob_base_velocity_smoother::geometryCallback, this, _1));
00096  
00097   // get parameters from parameter server if possible or write default values to variables
00098   if( !pnh_.hasParam("circular_buffer_capacity") )
00099   {
00100      ROS_WARN("No parameter circular_buffer_capacity on parameter server. Using default [12]");
00101   }
00102   pnh_.param("circular_buffer_capacity", buffer_capacity_, 12);
00103 
00104   if( !pnh_.hasParam("maximal_time_delay") )
00105   {
00106     ROS_WARN("No parameter maximal_time_delay on parameter server. Using default [4 in s]");
00107   }
00108   pnh_.param("maximal_time_delay", store_delay_, 4.0);
00109 
00110   if( !pnh_.hasParam("maximal_time_delay_to_stop") )
00111   {
00112     ROS_WARN("No parameter maximal_time_delay_to_stop on parameter server. Using default [0.1 in s]");
00113   }
00114   pnh_.param("maximal_time_delay_to_stop", stop_delay_after_no_sub_, 0.1);
00115   
00116   if( !pnh_.hasParam("thresh_max_acc") )
00117   {
00118     ROS_WARN("No parameter thresh_max_acc on parameter server. Using default [0.3 in m/s]");
00119   }
00120   pnh_.param("thresh_max_acc", acc_limit_, 0.3);
00121 
00122   if( !pnh_.hasParam("loop_rate") )
00123   {
00124     ROS_WARN("No parameter loop_rate on parameter server. Using default [30 in Hz]");
00125   }
00126   pnh_.param("loop_rate", loop_rate_, 30.0);
00127 
00128   double min_input_rate;
00129   if( !pnh_.hasParam("min_input_rate") )
00130   {
00131     ROS_WARN("No parameter min_input_rate on parameter server. Using default [9 in Hz]");
00132   }
00133   pnh_.param("min_input_rate", min_input_rate, 9.0);
00134 
00135   if( min_input_rate > loop_rate_)
00136   {
00137     ROS_WARN("min_input_rate > loop_rate: Setting min_input_rate to loop_rate = %f", loop_rate_);
00138     min_input_rate = loop_rate_;
00139   }
00140   max_delay_between_commands_ = 1/min_input_rate;
00141 
00142   // set a geometry message containing zero-values
00143   zero_values_.linear.x=0;
00144   zero_values_.linear.y=0;
00145   zero_values_.linear.z=0;
00146 
00147   zero_values_.angular.x=0;
00148   zero_values_.angular.y=0;
00149   zero_values_.angular.z=0;
00150 
00151   // initialize circular buffers
00152   cb_.set_capacity(buffer_capacity_);
00153   cb_out_.set_capacity(buffer_capacity_);
00154   cb_time_.set_capacity(buffer_capacity_);
00155 
00156   // set actual ros::Time
00157   ros::Time now = ros::Time::now();
00158 
00159   // fill circular buffer with zero values
00160   while(cb_.full() == false){
00161 
00162     cb_.push_front(zero_values_);
00163     cb_time_.push_front(now);
00164 
00165   }     
00166 };
00167 
00168 // destructor
00169 cob_base_velocity_smoother::~cob_base_velocity_smoother(){}
00170 
00171 // callback function to subsribe to the geometry messages cmd_vel and save them in a member variable
00172 void cob_base_velocity_smoother::geometryCallback(const geometry_msgs::Twist::ConstPtr &cmd_vel)
00173 {   
00174  
00175   sub_msg_ = *cmd_vel;
00176   set_new_msg_received(true);
00177  
00178 }
00179 
00180 // calculation function called periodically in main
00181 void cob_base_velocity_smoother::calculationStep(){
00182 
00183   // set current ros::Time
00184   ros::Time now = ros::Time::now();
00185   static ros::Time last = now;
00186 
00187   geometry_msgs::Twist result = geometry_msgs::Twist();
00188 
00189   // only publish command if we received a msg or the last message was actually zero
00190   if (get_new_msg_received())
00191   {
00192     // generate Output messages
00193     result = this->setOutput(now, sub_msg_);
00194     pub_.publish(result);
00195     
00196     last = now;
00197     set_new_msg_received(false);
00198   }
00199   // start writing in zeros if we did not receive a new msg within a certain amount of time.
00200   // Do not publish! Otherwise, the output of other nodes will be overwritten!
00201   else if ( fabs((last - now).toSec()) > max_delay_between_commands_)
00202     result = this->setOutput(now, geometry_msgs::Twist());
00203   // if last message was a zero msg, fill the buffer with zeros and publish again
00204   else if (IsZeroMsg(sub_msg_))
00205   {
00206     result = this->setOutput(now, sub_msg_);
00207     pub_.publish(result);
00208   }
00209 
00210 }
00211 
00212 // function for the actual computation
00213 // calls the reviseCircBuff and the meanValue-functions and limits the acceleration under thresh
00214 // returns the resulting geomtry message to be published to the base_controller
00215 geometry_msgs::Twist cob_base_velocity_smoother::setOutput(ros::Time now, geometry_msgs::Twist cmd_vel)
00216 {
00217   geometry_msgs::Twist result = zero_values_;
00218 
00219   // update the circular buffers
00220   this->reviseCircBuff(now, cmd_vel);
00221 
00222   // calculate the mean values for each direction
00223   result.linear.x = meanValueX();
00224   result.linear.y = meanValueY();
00225   result.angular.z = meanValueZ();
00226 
00227   // limit acceleration
00228   this->limitAcceleration(now, result);
00229 
00230   // insert the result-message into the circular-buffer storing the output
00231   cb_out_.push_front(result);
00232 
00233   return result;
00234 
00235 }
00236 
00237 // function that updates the circular buffer after receiving a new geometry message
00238 void cob_base_velocity_smoother::reviseCircBuff(ros::Time now, geometry_msgs::Twist cmd_vel)
00239 {
00240   if(this->circBuffOutOfDate(now) == true){
00241     // the circular buffer is out of date, so clear and refill with zero messages before adding the new command
00242 
00243     // clear buffers
00244     cb_.clear();
00245     cb_time_.clear();
00246 
00247     // fill circular buffer with zero_values_ and time buffer with actual time-stamp
00248     while(cb_.full() == false){
00249 
00250       cb_.push_front(zero_values_);
00251       cb_time_.push_front(now);
00252 
00253     }
00254 
00255     // add new command velocity message to circular buffer
00256     cb_.push_front(cmd_vel);
00257     // add new timestamp for subscribed command velocity message
00258     cb_time_.push_front(now);
00259 
00260   }
00261   else{
00262     // only some elements of the circular buffer are out of date, so only delete those
00263     double delay=(now.toSec() - cb_time_.back().toSec());
00264 
00265     while( delay >= store_delay_ ){
00266       // remove out-dated messages
00267       cb_.pop_back();
00268       cb_time_.pop_back();
00269 
00270       delay=(now.toSec() - cb_time_.back().toSec());
00271     }
00272     // if the circular buffer is empty now, refill with zero values
00273     if(cb_.empty() == true){
00274       while(cb_.full() == false){
00275 
00276         cb_.push_front(zero_values_);
00277         cb_time_.push_front(now);
00278 
00279       }
00280     }
00281     if(this->IsZeroMsg(cmd_vel)){
00282       // here we subscribed  a zero message, so we want to stop the robot
00283       long unsigned int size = floor( cb_.size() / 3 );
00284 
00285       // to stop the robot faster, fill the circular buffer with more than one, in fact floor (cb_.size() / 3 ), zero messages
00286       for(long unsigned int i=0; i< size; i++){
00287 
00288         // add new command velocity message to circular buffer
00289         cb_.push_front(cmd_vel);
00290         // add new timestamp for subscribed command velocity message
00291         cb_time_.push_front(now);
00292       }
00293 
00294     }
00295     else{
00296       // add new command velocity message to circular buffer
00297       cb_.push_front(cmd_vel);
00298       // add new timestamp for subscribed command velocity message
00299       cb_time_.push_front(now);
00300     }
00301   }
00302 };
00303 
00304 // returns true if all messages in cb are out of date in consideration of store_delay
00305 bool cob_base_velocity_smoother::circBuffOutOfDate(ros::Time now)
00306 {
00307   bool result=true;
00308 
00309   long unsigned int count=0;
00310 
00311   while( (count < cb_.size()) && (result == true) ){
00312                 
00313     double delay=(now.toSec() - cb_time_[count].toSec());
00314     if(delay < store_delay_){
00315       result = false;
00316     }
00317     count++;
00318   }
00319 
00320   return result;
00321 
00322 };
00323 
00324 // returns true if the input msg cmd_vel equals zero_values_, false otherwise
00325 bool cob_base_velocity_smoother::IsZeroMsg(geometry_msgs::Twist cmd_vel)
00326 {
00327   bool result = true;
00328   if( (cmd_vel.linear.x) != 0 || (cmd_vel.linear.y != 0) || (cmd_vel.angular.z != 0) ){
00329     result = false;
00330   }
00331 
00332   return result;
00333 };
00334 
00335 int cob_base_velocity_smoother::signum(double var)
00336 {
00337   if(var < 0){
00338     return -1;
00339   }
00340   else{
00341     return 1;
00342   }
00343 };
00344 
00345 // functions to calculate the mean values for linear/x
00346 double cob_base_velocity_smoother::meanValueX()
00347 {
00348   double result = 0;
00349   long unsigned int size = cb_.size();
00350 
00351   // calculate sum
00352   for(long unsigned int i=0; i<size; i++){
00353 
00354     result += cb_[i].linear.x;
00355 
00356   }
00357   result /= size;
00358         
00359   if(size > 1){
00360 
00361     double help_result = 0;
00362     double max = cb_[0].linear.x;
00363     long unsigned int max_ind = 0;
00364     for(long unsigned int i=0; i<size; i++){
00365 
00366       if(abs(result-cb_[i].linear.x) > abs(result-max)){
00367         max = cb_[i].linear.x;
00368         max_ind = i;
00369       }
00370 
00371     }
00372 
00373     // calculate sum
00374     for(long unsigned int i=0; i<size; i++){
00375                 
00376       if(i != max_ind){
00377         help_result += cb_[i].linear.x;
00378       }
00379     }
00380     result = help_result / (size - 1);
00381   }
00382 
00383   return result;
00384         
00385 };
00386 
00387 // functions to calculate the mean values for linear/y
00388 double cob_base_velocity_smoother::meanValueY()
00389 {
00390   double result = 0;
00391   long unsigned int size = cb_.size();
00392 
00393   // calculate sum
00394   for(long unsigned int i=0; i<size; i++){
00395 
00396     result += cb_[i].linear.y;
00397 
00398   }
00399   result /= size;
00400 
00401   if(size > 1){
00402 
00403     double help_result = 0;
00404     double max = cb_[0].linear.y;
00405     long unsigned int max_ind = 0;
00406     for(long unsigned int i=0; i<size; i++){
00407 
00408       if(abs(result-cb_[i].linear.y) > abs(result-max)){
00409         max = cb_[i].linear.y;
00410         max_ind = i;
00411       }
00412 
00413     }
00414 
00415     // calculate sum
00416     for(long unsigned int i=0; i<size; i++){
00417                 
00418       if(i != max_ind){
00419         help_result += cb_[i].linear.y;
00420       }
00421     }
00422     result = help_result / (size - 1);
00423   }
00424 
00425   return result;
00426         
00427 };
00428 
00429 // functions to calculate the mean values for angular/z
00430 double cob_base_velocity_smoother::meanValueZ()
00431 {
00432   double result = 0;
00433   long unsigned int size = cb_.size();
00434 
00435   // calculate sum
00436   for(long unsigned int i=0; i<size; i++){
00437 
00438   result += cb_[i].angular.z;
00439 
00440   }
00441   result /= size;
00442         
00443   if(size > 1){
00444                 
00445     double help_result = 0;
00446     double max = cb_[0].angular.z;
00447     long unsigned int max_ind = 0;
00448     for(long unsigned int i=0; i<size; i++){
00449 
00450       if(abs(result-cb_[i].angular.z) > abs(result-max)){
00451         max = cb_[i].angular.z;
00452         max_ind = i;
00453       }
00454 
00455     }
00456 
00457     // calculate sum
00458     for(long unsigned int i=0; i<size; i++){
00459 
00460       if(i != max_ind){
00461         help_result += cb_[i].angular.z;
00462       }
00463     }
00464     result = help_result / (size - 1);
00465   }
00466 
00467   return result;
00468         
00469 };
00470 
00471 // function to make the loop rate availabe outside the class
00472 double cob_base_velocity_smoother::getLoopRate(){
00473 
00474   return loop_rate_;
00475 
00476 }
00477 
00478 // function to compare two geometry messages
00479 bool cob_base_velocity_smoother::IsEqual(geometry_msgs::Twist msg1, geometry_msgs::Twist msg2){
00480 
00481   if( (msg1.linear.x == msg2.linear.x) && (msg1.linear.y == msg2.linear.y) && (msg1.angular.z == msg2.angular.z)){
00482     return true;
00483   }else
00484   {
00485     return false;
00486   }
00487 
00488 }
00489 
00490 //function to limit the acceleration under the given threshhold thresh
00491 void cob_base_velocity_smoother::limitAcceleration(ros::Time now, geometry_msgs::Twist& result){
00492 
00493   // limit the acceleration under thresh
00494   // only if cob_base_velocity_smoother has published a message yet
00495         
00496   double deltaTime = 0; 
00497 
00498   if(cb_time_.size() > 1){
00499     deltaTime = now.toSec() - cb_time_[2].toSec();
00500   }
00501 
00502   if( cb_out_.size() > 0){
00503 
00504     if(deltaTime > 0){
00505       // set delta velocity and acceleration values
00506       double deltaX = result.linear.x - cb_out_.front().linear.x;
00507 
00508       double deltaY = result.linear.y - cb_out_.front().linear.y;
00509                 
00510       double deltaZ = result.angular.z - cb_out_.front().angular.z;
00511 
00512       if( abs(deltaX) > acc_limit_){
00513 
00514         result.linear.x = cb_out_.front().linear.x + this->signum(deltaX) * acc_limit_;
00515 
00516       }
00517       if( abs(deltaY) > acc_limit_){
00518 
00519         result.linear.y = cb_out_.front().linear.y + this->signum(deltaY) * acc_limit_;
00520 
00521       }
00522       if( abs(deltaZ) > acc_limit_){
00523 
00524         result.angular.z = cb_out_.front().angular.z + this->signum(deltaZ) * acc_limit_;
00525 
00526       }
00527     }
00528   }
00529 };
00530 
00531 
00532 int main(int argc, char **argv)
00533 {
00534   // initialize ros and specifiy node name
00535   ros::init(argc, argv, "cob_base_velocity_smoother");
00536 
00537   // create Node Class
00538   cob_base_velocity_smoother my_velocity_smoother;
00539   // get loop rate from class member
00540   ros::Rate rate(my_velocity_smoother.getLoopRate());
00541   // actual calculation step with given frequency
00542   while(my_velocity_smoother.nh_.ok()){
00543 
00544     my_velocity_smoother.calculationStep();
00545 
00546     ros::spinOnce();
00547     rate.sleep();
00548 
00549   }
00550 
00551   return 0;
00552 }


cob_base_velocity_smoother
Author(s): Florian Mirus
autogenerated on Tue Mar 3 2015 15:12:14