change_limiter.h
Go to the documentation of this file.
00001 
00026 #ifndef GRIZZLY_MOTION_CHANGE_LIMITER_H
00027 #define GRIZZLY_MOTION_CHANGE_LIMITER_H
00028 
00029 template<class Msg>
00030 class ChangeLimiter
00031 {
00032 public:
00033   ChangeLimiter(double max_change_per_second, float Msg::*field_ptr)
00034     : max_change_per_second_(max_change_per_second), field_(field_ptr)
00035   {
00036   }
00037 
00038   void apply(const Msg* msg_in, Msg* msg_out)
00039   {
00040     double diff_secs = (msg_in->header.stamp - last_msg_.header.stamp).toSec();
00041     if (diff_secs <= 0) {
00042       ROS_ERROR("Change limiter passed sequential drive messages with zero or negative timestamp differential.");
00043       return;
00044     }
00045     if (diff_secs > 0.1) {
00046       // If time difference is too great, make output field zero.
00047       msg_out->*field_ = 0;
00048     } else {
00049       double diff_value = msg_in->*field_ - last_msg_.*field_;
00050       double max_change = max_change_per_second_ * diff_secs;
00051       if (fabs(diff_value) < max_change) {
00052         msg_out->*field_ = msg_in->*field_;
00053       } else if (diff_value > 0) {
00054         msg_out->*field_ = last_msg_.*field_ + max_change;
00055       } else {
00056         msg_out->*field_ = last_msg_.*field_ - max_change;
00057       }
00058     }
00059     last_msg_ = *msg_out;
00060   }
00061 
00062   void setMaxChange(double max_change_per_second)
00063   {
00064     max_change_per_second_ = max_change_per_second;
00065   }
00066 
00067 protected:
00068   double max_change_per_second_;
00069   float Msg::*field_;
00070   Msg last_msg_;
00071 };
00072 
00073 #endif


grizzly_motion
Author(s):
autogenerated on Fri Aug 28 2015 10:55:30