change_limiter.h

Go to the documentation of this file.

#ifndef GRIZZLY_MOTION_CHANGE_LIMITER_H
#define GRIZZLY_MOTION_CHANGE_LIMITER_H

template<class Msg>
class ChangeLimiter
{
public:
  ChangeLimiter(double max_change_per_second, float Msg::*field_ptr)
    : max_change_per_second_(max_change_per_second), field_(field_ptr)
  {
  }

  void apply(const Msg* msg_in, Msg* msg_out)
  {
    double diff_secs = (msg_in->header.stamp - last_msg_.header.stamp).toSec();
    if (diff_secs <= 0) {
      ROS_ERROR("Change limiter passed sequential drive messages with zero or negative timestamp differential.");
      return;
    }
    if (diff_secs > 0.1) {
      // If time difference is too great, make output field zero.
      msg_out->*field_ = 0;
    } else {
      double diff_value = msg_in->*field_ - last_msg_.*field_;
      double max_change = max_change_per_second_ * diff_secs;
      if (fabs(diff_value) < max_change) {
        msg_out->*field_ = msg_in->*field_;
      } else if (diff_value > 0) {
        msg_out->*field_ = last_msg_.*field_ + max_change;
      } else {
        msg_out->*field_ = last_msg_.*field_ - max_change;
      }
    }
    last_msg_ = *msg_out;
  }

  void setMaxChange(double max_change_per_second)
  {
    max_change_per_second_ = max_change_per_second;
  }

protected:
  double max_change_per_second_;
  float Msg::*field_;
  Msg last_msg_;
};

#endif