path_interpolator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <list>
31 
35 
36 namespace planner_cspace
37 {
38 namespace planner_3d
39 {
40 std::list<CyclicVecFloat<3, 2>> PathInterpolator::interpolate(
41  const std::list<CyclicVecInt<3, 2>>& path_grid,
42  const float interval,
43  const int local_range) const
44 {
45  CyclicVecInt<3, 2> p_prev(0, 0, 0);
46  bool init = false;
47 
48  std::list<CyclicVecFloat<3, 2>> path;
49 
50  for (auto p : path_grid)
51  {
52  p.cycleUnsigned(angle_);
53 
54  if (init)
55  {
56  const CyclicVecInt<3, 2> ds = path_grid.front() - p;
57  CyclicVecInt<3, 2> d = p - p_prev;
58  d.cycle(angle_);
59  const CyclicVecInt<3, 2> d2(d[0] + range_, d[1] + range_, p[2]);
60 
61  const float inter = interval / d.len();
62 
63  if (d[0] == 0 && d[1] == 0)
64  {
65  const int yaw_inc = std::copysign(1, d[2]);
66  for (int yaw = p_prev[2]; yaw != p_prev[2] + d[2]; yaw += yaw_inc)
67  {
68  path.push_back(CyclicVecFloat<3, 2>(p[0], p[1], yaw));
69  }
70  }
71  else if (d[2] == 0 || ds.sqlen() > local_range * local_range)
72  {
73  for (float i = 0; i < 1.0; i += inter)
74  {
75  const float x2 = p_prev[0] * (1 - i) + p[0] * i;
76  const float y2 = p_prev[1] * (1 - i) + p[1] * i;
77  const float yaw2 = p_prev[2] + i * d[2];
78 
79  path.push_back(CyclicVecFloat<3, 2>(x2, y2, yaw2));
80  }
81  }
82  else
83  {
84  const auto& radiuses = rot_cache_.getRadiuses(p_prev[2], d2);
85  const float r1 = radiuses.first;
86  const float r2 = radiuses.second;
87  const float yawf = p[2] * M_PI * 2.0 / angle_;
88  const float yawf_prev = p_prev[2] * M_PI * 2.0 / angle_;
89 
90  const float cx = p[0] + r2 * cosf(yawf + M_PI / 2);
91  const float cy = p[1] + r2 * sinf(yawf + M_PI / 2);
92  const float cx_prev = p_prev[0] + r1 * cosf(yawf_prev + M_PI / 2);
93  const float cy_prev = p_prev[1] + r1 * sinf(yawf_prev + M_PI / 2);
94 
95  for (float i = 0; i < 1.0; i += inter)
96  {
97  const float r = r1 * (1.0 - i) + r2 * i;
98  const float cx2 = cx_prev * (1.0 - i) + cx * i;
99  const float cy2 = cy_prev * (1.0 - i) + cy * i;
100  const float cyaw = p_prev[2] + i * d[2];
101  const float cyawf = cyaw * M_PI * 2.0 / angle_;
102 
103  const float x2 = cx2 - r * cosf(cyawf + M_PI / 2);
104  const float y2 = cy2 - r * sinf(cyawf + M_PI / 2);
105 
106  path.push_back(CyclicVecFloat<3, 2>(x2, y2, cyaw));
107  }
108  }
109  }
110  p_prev = p;
111  init = true;
112  }
113  path.push_back(CyclicVecFloat<3, 2>(path_grid.back()));
114  return path;
115 }
116 } // namespace planner_3d
117 } // namespace planner_cspace
d
Chain d2()
void cycle(const int res, const ArgList &...rest)
Definition: cyclic_vec.h:282
std::list< CyclicVecFloat< 3, 2 > > interpolate(const std::list< CyclicVecInt< 3, 2 >> &path_grid, const float interval, const int local_range) const
const std::pair< float, float > & getRadiuses(const int start_angle, const CyclicVecInt< 3, 2 > &end) const


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:42