one_d_velocity_iterator.hpp

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Locus Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
 
#ifndef DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
#define DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
 
#include <algorithm>
#include <cmath>
 
namespace dwb_plugins
{
 
const double EPSILON = 1E-5;
 
inline double projectVelocity(double v0, double accel, double decel, double dt, double target)
{
  double v1;
  if (v0 < target) {
    v1 = v0 + accel * dt;
    return std::min(target, v1);
  } else {
    v1 = v0 + decel * dt;
    return std::max(target, v1);
  }
}
 
class OneDVelocityIterator
{
public:
  OneDVelocityIterator(
    double current, double min, double max, double acc_limit, double decel_limit, double acc_time,
    int num_samples)
  {
    if (current < min) {
      current = min;
    } else if (current > max) {
      current = max;
    }
    max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max);
    min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min);
    reset();
 
    if (fabs(min_vel_ - max_vel_) < EPSILON) {
      increment_ = 1.0;
      return;
    }
    num_samples = std::max(2, num_samples);
 
    // e.g. for 4 samples, split distance in 3 even parts
    increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1));
  }
 
  double getVelocity() const
  {
    if (return_zero_now_) {return 0.0;}
    return current_;
  }
 
  OneDVelocityIterator & operator++()
  {
    if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 &&
      current_ + increment_ <= max_vel_ + EPSILON)
    {
      return_zero_now_ = true;
      return_zero_ = false;
    } else {
      current_ += increment_;
      return_zero_now_ = false;
    }
    return *this;
  }
 
  void reset()
  {
    current_ = min_vel_;
    return_zero_ = true;
    return_zero_now_ = false;
  }
 
  bool isFinished() const
  {
    return current_ > max_vel_ + EPSILON;
  }
 
private:
  bool return_zero_, return_zero_now_;
  double min_vel_, max_vel_;
  double current_;
  double increment_;
};
}  // namespace dwb_plugins
 
#endif  // DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_