cob_base_velocity_smoother.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #ifndef COB_BASE_VELOCITY_SMOOTHER_H
00019 #define COB_BASE_VELOCITY_SMOOTHER_H
00020 
00021 //**************************** includes ************************/
00022 
00023 // standard includes
00024 #include <XmlRpc.h>
00025 #include <pthread.h>
00026 #include <deque>
00027 #include <sstream>
00028 #include <iostream>
00029 #include <boost/circular_buffer.hpp>
00030 #include <boost/bind.hpp>
00031 
00032 // ros includes
00033 #include <ros/ros.h>
00034 #include <geometry_msgs/Twist.h>
00035 #include <ros/console.h>
00036 #include <std_msgs/String.h>
00037 /****************************************************************
00038  * the ros navigation doesn't run very smoothly because acceleration is too high
00039  * --> cob has strong base motors and therefore reacts with shaking behavior
00040  * (PR2 has much more mechanical damping)
00041  * solution: the additional node cob_base_velocity_smoother smooths the velocities
00042  * comming from ROS-navigation or teleoperation, by calculating the mean values of a certain number
00043  * of past messages and limiting the acceleration under a given threshold.
00044  * cob_base_velocity_smoother subsribes (input) and publishes (output) geometry_msgs::Twist.
00045  ****************************************************************/
00046 class cob_base_velocity_smoother
00047 {
00048 private:
00049   //capacity for circular buffers (to be loaded from parameter server, otherwise set to default value 12)
00050   int buffer_capacity_;
00051   //maximal time-delay in seconds for stored messages in Circular Buffer (to be loaded from parameter server, otherwise set to default value 4)
00052   double store_delay_;
00053   // maximal time-delay in seconds to wait until filling the circular buffer with zero messages when the subscriber doesn't hear anything
00054   double stop_delay_after_no_sub_;
00055   //threshhold for maximal allowed acceleration (to be loaded from parameter server, otherwise set to default value 0.02)
00056   double acc_limit_;
00057   // ros loop rate (to be loaded from parameter server, otherwise set to default value 30)
00058   double loop_rate_;
00059   // delay between received commands that is allowed. After that, fill buffer with zeros.
00060   double max_delay_between_commands_;
00061   //geometry message filled with zero values
00062   geometry_msgs::Twist zero_values_;
00063   // subscribed geometry message
00064   geometry_msgs::Twist sub_msg_;
00065 
00066   pthread_mutex_t m_mutex;
00067   bool new_msg_received_;
00068 
00069   // function for checking wether a new msg has been received, triggering publishers accordingly
00070   void set_new_msg_received(bool received);
00071   bool get_new_msg_received();
00072 
00073 public:
00074   // constructor
00075   cob_base_velocity_smoother();
00076 
00077   // destructor
00078   ~cob_base_velocity_smoother();
00079 
00080   //create node handle
00081   ros::NodeHandle nh_, pnh_;
00082 
00083   //circular buffers for velocity, output and time
00084   boost::circular_buffer<geometry_msgs::Twist> cb_;
00085   boost::circular_buffer<geometry_msgs::Twist> cb_out_;
00086   boost::circular_buffer<ros::Time> cb_time_;
00087 
00088   // declaration of ros subscribers
00089   ros::Subscriber geometry_msgs_sub_;
00090 
00091   // decalaration of ros publishers
00092   ros::Publisher pub_;
00093 
00094   //callback function to subsribe to the geometry messages
00095   void geometryCallback(const geometry_msgs::Twist::ConstPtr &cmd_vel);
00096   //calculation function called periodically in main
00097   void calculationStep();
00098   //function that updates the circular buffer after receiving a new geometry message
00099   void reviseCircBuff(ros::Time now, geometry_msgs::Twist cmd_vel);
00100   //function to limit the acceleration under the given threshhold thresh
00101   void limitAcceleration(ros::Time now, geometry_msgs::Twist& cmd_vel);
00102 
00103   //boolean function that returns true if all messages stored in the circular buffer are older than store_delay, false otherwise
00104   bool circBuffOutOfDate(ros::Time now);
00105   // function to compare two geometry messages
00106   bool IsEqual(geometry_msgs::Twist msg1, geometry_msgs::Twist msg2);
00107 
00108   //boolean function that returns true if the input msg cmd_vel equals zero_values, false otherwise
00109   bool IsZeroMsg(geometry_msgs::Twist cmd_vel);
00110 
00111   //help-function that returns the signum of a double variable
00112   int signum(double var);
00113 
00114   //functions to calculate the mean values for each direction
00115   double meanValueX();
00116   double meanValueY();
00117   double meanValueZ();
00118 
00119   // function to make the loop rate available outside the class
00120   double getLoopRate();
00121 
00122   //function for the actual computation
00123   //calls the reviseCircBuff and the meanValue-functions and limits the acceleration under thresh
00124   //returns the resulting geometry message to be published to the base_controller
00125   geometry_msgs::Twist setOutput(ros::Time now, geometry_msgs::Twist cmd_vel);
00126 };
00127 
00128 #endif


cob_base_velocity_smoother
Author(s): Florian Mirus , Benjamin Maidel
autogenerated on Thu Jun 6 2019 21:18:55