one_d_velocity_iterator.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H
36 #define DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H
37 
38 #include <algorithm>
39 #include <cmath>
40 
41 namespace dwb_plugins
42 {
43 
44 const double EPSILON = 1E-5;
45 
56 inline double projectVelocity(double v0, double accel, double decel, double dt, double target)
57 {
58  double v1;
59  if (v0 < target)
60  {
61  v1 = v0 + accel * dt;
62  return std::min(target, v1);
63  }
64  else
65  {
66  v1 = v0 + decel * dt;
67  return std::max(target, v1);
68  }
69 }
70 
86 {
87 public:
98  OneDVelocityIterator(double current, double min, double max, double acc_limit, double decel_limit, double acc_time,
99  int num_samples)
100  {
101  if (current < min)
102  {
103  current = min;
104  }
105  else if (current > max)
106  {
107  current = max;
108  }
109  max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max);
110  min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min);
111  reset();
112 
113  if (fabs(min_vel_ - max_vel_) < EPSILON)
114  {
115  increment_ = 1.0;
116  return;
117  }
118  num_samples = std::max(2, num_samples);
119 
120  // e.g. for 4 samples, split distance in 3 even parts
121  increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1));
122  }
123 
127  double getVelocity() const
128  {
129  if (return_zero_now_) return 0.0;
130  return current_;
131  }
132 
137  {
138  if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 && current_ + increment_ <= max_vel_ + EPSILON)
139  {
140  return_zero_now_ = true;
141  return_zero_ = false;
142  }
143  else
144  {
145  current_ += increment_;
146  return_zero_now_ = false;
147  }
148  return *this;
149  }
150 
154  void reset()
155  {
156  current_ = min_vel_;
157  return_zero_ = true;
158  return_zero_now_ = false;
159  }
160 
164  bool isFinished() const
165  {
166  return current_ > max_vel_ + EPSILON;
167  }
168 
169 private:
172  double current_;
173  double increment_;
174 };
175 } // namespace dwb_plugins
176 
177 #endif // DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H
const double EPSILON
An iterator for generating a number of samples in a range.
double getVelocity() const
Get the next velocity available.
OneDVelocityIterator & operator++()
Increment the iterator.
void reset()
Reset back to the first velocity.
OneDVelocityIterator(double current, double min, double max, double acc_limit, double decel_limit, double acc_time, int num_samples)
Constructor for the velocity iterator.
double projectVelocity(double v0, double accel, double decel, double dt, double target)
Given initial conditions and a time, figure out the end velocity.


dwb_plugins
Author(s):
autogenerated on Wed Jun 26 2019 20:06:16