cob_vel_integrator.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_navigation
00012  * ROS package name: cob_vel_integrator
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 #include <ros/ros.h>
00051 #include <XmlRpc.h>
00052 #include <pthread.h>
00053 #include <std_msgs/String.h>
00054 #include <geometry_msgs/Twist.h>
00055 #include <ros/console.h>
00056 #include <deque>
00057 #include <sstream>
00058 #include <iostream>
00059 #include <boost/circular_buffer.hpp>
00060 #include <boost/bind.hpp>
00061 
00062 using namespace std;
00063 
00064 /****************************************************************
00065  * the ros navigation doesn't run very smoothly because acceleration is too high
00066  * --> cob has strong base motors and therefore reacts with shaking behavior 
00067  * (PR2 has much more meachnical damping)
00068  * solution: the additional node cob_vel_integrator smooths the velocities
00069  * comming from ROS-navigation, by calculating the mean values of a certain number 
00070  * of past messages and limiting the acceleration under a given threshold. 
00071  * cob_vel_integrator subsribes (input) and publishes (output) geometry_msgs::Twist.
00072  ****************************************************************/
00073 class cob_vel_integrator
00074 {
00075 private:
00076         //capacity for circular buffers (to be loaded from parameter server, otherwise set to default value 12)
00077         int buffer_capacity;
00078         //maximal time-delay in seconds for stored messages in Circular Buffer (to be loaded from parameter server, otherwise set to default value 4)
00079         double store_delay;
00080         //threshhold for maximal allowed acceleration (to be loaded from parameter server, otherwise set to default value 0.02)
00081         double thresh;
00082         //geometry message filled with zero values
00083         geometry_msgs::Twist zero_values;
00084 
00085 public:
00086         
00087         //constructor
00088         cob_vel_integrator();
00089 
00090         //create node handle
00091         ros::NodeHandle n;
00092 
00093         //circular buffers for velocity, acceleration and time
00094         boost::circular_buffer<geometry_msgs::Twist> cb;
00095         boost::circular_buffer<geometry_msgs::Twist> cb_out;
00096         boost::circular_buffer<ros::Time> cb_time;
00097 
00098         //ros publisher
00099         ros::Publisher pub;
00100 
00101         //callback function to subsribe to the geometry messages cmd_vel and publish to base_controller/command
00102         void geometryCallback(const geometry_msgs::Twist& cmd_vel);
00103         //function that updates the circular buffer after receiving a new geometry message
00104         void reviseCircBuff(ros::Time now, geometry_msgs::Twist cmd_vel);
00105         
00106         //boolean function that returns true if all messages stored in the circular buffer are older than store_delay,
00107         //false otherwise
00108         bool CircBuffOutOfDate(ros::Time now);
00109 
00110         //functions to calculate the mean values for each direction
00111         double meanValueX();
00112         double meanValueY();
00113         double meanValueZ();
00114 
00115         //function for the actual computation
00116         //calls the reviseCircBuff and the meanValue-functions and limits the acceleration under thresh
00117         //returns the resulting geometry message to be published to the base_controller
00118         geometry_msgs::Twist setOutput(geometry_msgs::Twist cmd_vel);
00119 
00120 };
00121 
00122 //constructor
00123 cob_vel_integrator::cob_vel_integrator()
00124 {
00125         
00126         //get parameters from parameter server if possible or take default values
00127         if(n.hasParam("/cob_vel_integrator/circular_buffer_capacity"))
00128         {
00129                 n.getParam("/cob_vel_integrator/circular_buffer_capacity",buffer_capacity);
00130         }
00131         else
00132         {
00133                 buffer_capacity = 12;
00134                 ROS_WARN("Used default parameter for circular buffer capacity [12]");
00135         }
00136 
00137         if(n.hasParam("/cob_vel_integrator/maximal_time_delay"))
00138         {
00139                 n.getParam("/cob_vel_integrator/maximal_time_delay",store_delay);
00140         }
00141         else
00142         {
00143                 store_delay = 4;
00144                 ROS_WARN("Used default parameter for maximal time delay in seconds for saved messages [4]");
00145         }
00146 
00147         if(n.hasParam("/cob_vel_integrator/thresh_max_acc"))
00148         {
00149                 n.getParam("/cob_vel_integrator/thresh_max_acc",thresh);
00150         }
00151 
00152         else
00153         {
00154                 thresh = 0.02;
00155                 ROS_WARN("Used default parameter for maximal allowed acceleration in m per s [0.02]");
00156         }
00157 
00158         //set a geometry message containing zero-values
00159         zero_values.linear.x=0;
00160         zero_values.linear.y=0;
00161         zero_values.linear.z=0;
00162 
00163         zero_values.angular.x=0;
00164         zero_values.angular.y=0;
00165         zero_values.angular.z=0;
00166 
00167         //initialize circular buffers
00168         cb.set_capacity(buffer_capacity);
00169         cb_out.set_capacity(buffer_capacity);
00170         cb_time.set_capacity(buffer_capacity);
00171         
00172         //set actual ros::Time
00173         ros::Time now=ros::Time::now();
00174 
00175         //fill circular buffer with zero values
00176         while(cb.full() == false){
00177 
00178                 cb.push_front(zero_values);
00179                 cb_time.push_front(now);
00180 
00181         }
00182 
00183         pub = n.advertise<geometry_msgs::Twist>("output", 1);
00184         
00185 };
00186 
00187 //returns true if all messages in cb are out of date in consideration of store_delay
00188 bool cob_vel_integrator::CircBuffOutOfDate(ros::Time now)
00189 {
00190         bool result=true;
00191 
00192         long unsigned int count=0;
00193 
00194         while( (count < cb.size()) && (result == true) ){
00195                 
00196                 double delay=(now.toSec() - cb_time[count].toSec());
00197 
00198                 if(delay < store_delay){
00199                         result = false;
00200                 }
00201                 count++;
00202         }
00203 
00204         return result;
00205 
00206 };
00207 
00208 //functions to calculate the mean values for linear/x
00209 double cob_vel_integrator::meanValueX()
00210 {
00211         double result = 0;
00212         long unsigned int size = cb.size();
00213 
00214         //calculate sum
00215         for(long unsigned int i=0; i<size; i++){
00216 
00217                 result = result + cb[i].linear.x;
00218 
00219         }
00220         result = result / size;
00221 
00222         if(size > 1){
00223                 double max = cb[0].linear.x;
00224                 long unsigned int max_ind = 0;
00225                 for(long unsigned int i=0; i<size; i++){
00226 
00227                         if(abs(result-cb[i].linear.x) > abs(result-max)){
00228                                 max = cb[i].linear.x;
00229                                 max_ind = i;
00230                         }
00231 
00232                 }
00233 
00234                 //calculate sum
00235                 for(long unsigned int i=0; i<size; i++){
00236                 
00237                         if(i != max_ind){
00238                                 result = result + cb[i].linear.x;
00239                         }
00240                 }
00241                 result = result / (size - 1);
00242         }
00243         
00244         return result;
00245         
00246 };
00247 
00248 //functions to calculate the mean values for linear/y
00249 double cob_vel_integrator::meanValueY()
00250 {
00251         double result = 0;
00252         long unsigned int size = cb.size();
00253 
00254         //calculate sum
00255         for(long unsigned int i=0; i<size; i++){
00256 
00257                 result = result + cb[i].linear.y;
00258 
00259         }
00260         result = result / size;
00261 
00262         if(size > 1){
00263 
00264                 double max = cb[0].linear.y;
00265                 long unsigned int max_ind = 0;
00266                 for(long unsigned int i=0; i<size; i++){
00267 
00268                         if(abs(result-cb[i].linear.y) > abs(result-max)){
00269                                 max = cb[i].linear.y;
00270                                 max_ind = i;
00271                         }
00272 
00273                 }
00274 
00275                 //calculate sum
00276                 for(long unsigned int i=0; i<size; i++){
00277                 
00278                         if(i != max_ind){
00279                                 result = result + cb[i].linear.y;
00280                         }
00281                 }
00282                 result = result / (size - 1);
00283         }
00284 
00285         return result;
00286         
00287 };
00288 
00289 //functions to calculate the mean values for angular/z
00290 double cob_vel_integrator::meanValueZ()
00291 {
00292         double result = 0;
00293         long unsigned int size = cb.size();
00294 
00295         //calculate sum
00296         for(long unsigned int i=0; i<size; i++){
00297 
00298                 result = result + cb[i].angular.z;
00299 
00300         }
00301         result = result / size;
00302 
00303         if(size > 1){
00304 
00305                 double max = cb[0].angular.z;
00306                 long unsigned int max_ind = 0;
00307                 for(long unsigned int i=0; i<size; i++){
00308 
00309                         if(abs(result-cb[i].angular.z) > abs(result-max)){
00310                                 max = cb[i].angular.z;
00311                                 max_ind = i;
00312                         }
00313 
00314                 }
00315 
00316                 //calculate sum
00317                 for(long unsigned int i=0; i<size; i++){
00318                 
00319                         if(i != max_ind){
00320                                 result = result + cb[i].angular.z;
00321                         }
00322                 }
00323                 result = result / (size - 1);
00324 
00325         }
00326 
00327         return result;
00328         
00329 };
00330 
00331 //function that updates the circular buffer after receiving a new geometry message
00332 void cob_vel_integrator::reviseCircBuff(ros::Time now, geometry_msgs::Twist cmd_vel)
00333 {
00334         if(this->CircBuffOutOfDate(now) == true){
00335 
00336                 //clear buffers
00337                 cb.clear();
00338                 cb_time.clear();
00339 
00340                 //fill circular buffer with zero_values and time buffer with actual time-stamp
00341                 while(cb.full() == false){
00342 
00343                         cb.push_front(zero_values);
00344                         cb_time.push_front(now);
00345 
00346                 }
00347 
00348                 //add new command velocity message to circular buffer
00349                 cb.push_front(cmd_vel);
00350                 //add new timestamp for subscribed command velocity message
00351                 cb_time.push_front(now);
00352 
00353         }
00354         else{
00355                 //cout << "revise CircBuff: else-case!" << endl;
00356                 double delay=(now.toSec() - cb_time.back().toSec());
00357 
00358                 while( delay >= store_delay ){
00359                         //remove out-dated messages
00360                         cb.pop_back();
00361                         cb_time.pop_back();
00362         
00363                         delay=(now.toSec() - cb_time.back().toSec());
00364                 }
00365 
00366                 //add new command velocity message to circular buffer
00367                 cb.push_front(cmd_vel);
00368                 //add new timestamp for subscribed command velocity message
00369                 cb_time.push_front(now);
00370 
00371         }
00372 };
00373 
00374 
00375 //function for the actual computation
00376 //calls the reviseCircBuff and the meanValue-functions and limits the acceleration under thresh
00377 //returns the resulting geometry message to be published to the base_controller
00378 geometry_msgs::Twist cob_vel_integrator::setOutput(geometry_msgs::Twist cmd_vel)
00379 {
00380         geometry_msgs::Twist result = zero_values;
00381         
00382         //set actual ros::Time
00383         ros::Time now=ros::Time::now();
00384         //update the circular buffers
00385         this->reviseCircBuff(now, cmd_vel);
00386 
00387         //calculate the mean values for each direction
00388         result.linear.x = meanValueX();
00389         result.linear.y = meanValueY();
00390         result.angular.z = meanValueZ();
00391 
00392         //limit the acceleration under thresh
00393         // only if cob_vel_integrator has published a message yet
00394         if( cb_out.size() > 1){
00395         
00396                 //set delty velocity and acceleration values
00397                 double deltaX = result.linear.x - cb_out.front().linear.x;
00398                 double accX = deltaX / ( now.toSec() - cb_time.front().toSec() );
00399 
00400                 double deltaY = result.linear.y - cb_out.front().linear.y;
00401                 double accY = deltaY / ( now.toSec() - cb_time.front().toSec() );
00402 
00403                 double deltaZ = result.angular.z - cb_out.front().linear.y;
00404                 double accZ = deltaZ /  ( now.toSec() - cb_time.front().toSec() );
00405 
00406                 if( abs(accX) > thresh){
00407                         result.linear.x = cb_out.front().linear.x + ( thresh *  ( now.toSec() - cb_time.front().toSec() ) );
00408                 }
00409                 if( abs(accY) > thresh){
00410                         result.linear.y = cb_out.front().linear.y + ( thresh *  ( now.toSec() - cb_time.front().toSec() ));
00411                 }
00412                 if( abs(accZ) > thresh){
00413                         result.angular.z = cb_out.front().angular.z + ( thresh *  ( now.toSec() - cb_time.front().toSec() ) );
00414                 }
00415 
00416                 cb_out.push_front(result);
00417         }
00418 
00419         return result;
00420 
00421 }
00422 
00423 //callback function to subsribe to the geometry messages cmd_vel and publish to base_controller/command
00424 void cob_vel_integrator::geometryCallback(const geometry_msgs::Twist& cmd_vel)
00425 {
00426 
00427         //ROS_INFO("%s","I heard something, so here's the callBack-Function!");
00428         //cout << "subscribed-message: " << cmd_vel << endl;
00429         
00430         //generate Output messages
00431         geometry_msgs::Twist result = this->setOutput(cmd_vel);
00432 
00433         //print result
00434         //cout << "result message: " << result << endl;
00435 
00436         //publish result
00437         pub.publish(result);
00438 
00439 };
00440 
00441 int main(int argc, char **argv)
00442 {
00443 
00444         ros::init(argc, argv, "cob_vel_integrator");
00445         
00446         cob_vel_integrator my_cvi = cob_vel_integrator();
00447 
00448         ros::Subscriber sub=my_cvi.n.subscribe("input", 1, &cob_vel_integrator::geometryCallback, &my_cvi);
00449         
00450         ros::spin();
00451 
00452         return 0;
00453 }
00454 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


cob_vel_integrator
Author(s): Florian Mirus
autogenerated on Mon Mar 11 2013 13:23:37