test_path2d.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, 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 <algorithm>
31 #include <cmath>
32 #include <cstddef>
33 #include <limits>
34 #include <string>
35 #include <vector>
36 
37 #include <gtest/gtest.h>
38 
39 #include <nav_msgs/Path.h>
40 #include <trajectory_tracker/eigen_line.h>
41 #include <trajectory_tracker/path2d.h>
42 #include <trajectory_tracker_msgs/PathWithVelocity.h>
43 
44 namespace
45 {
46 double getRemainedDistance(const trajectory_tracker::Path2D& path, const Eigen::Vector2d& p)
47 {
48  const auto nearest = path.findNearest(path.begin(), path.end(), p);
49  const Eigen::Vector2d pos_on_line =
50  trajectory_tracker::projection2d((nearest - 1)->pos_, nearest->pos_, p);
51  return path.remainedDistance(path.begin(), nearest, path.end(), pos_on_line);
52 }
53 } // namespace
54 
55 TEST(Path2D, RemainedDistance)
56 {
57  trajectory_tracker::Path2D path_full;
58  for (double x = 0.0; x < 10.0 - 1e-3; x += 0.2)
59  path_full.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(x, 0), 0, 1));
60  path_full.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(10.0, 0), 0, 1));
61 
62  // Single pose path means orientation control mode. More than two poses should be line following.
63  for (size_t l = 2; l < path_full.size(); ++l)
64  {
65  trajectory_tracker::Path2D path;
66  path.resize(l);
67  std::copy(path_full.end() - l, path_full.end(), path.begin());
68 
69  std::string err_msg = "failed for " + std::to_string(l) + " pose(s) path";
70 
71  ASSERT_NEAR(getRemainedDistance(path, Eigen::Vector2d(0, 0)), 10.0, 1e-2) << err_msg;
72 
73  ASSERT_NEAR(getRemainedDistance(path, Eigen::Vector2d(8.25, 0)), 1.75, 1e-2) << err_msg;
74  ASSERT_NEAR(getRemainedDistance(path, Eigen::Vector2d(10.25, 0)), -0.25, 1e-2) << err_msg;
75 
76  ASSERT_NEAR(getRemainedDistance(path, Eigen::Vector2d(8.25, 0.1)), 1.75, 1e-2) << err_msg;
77  ASSERT_NEAR(getRemainedDistance(path, Eigen::Vector2d(8.25, -0.1)), 1.75, 1e-2) << err_msg;
78  ASSERT_NEAR(getRemainedDistance(path, Eigen::Vector2d(10.25, 0.1)), -0.25, 1e-2) << err_msg;
79  ASSERT_NEAR(getRemainedDistance(path, Eigen::Vector2d(10.25, -0.1)), -0.25, 1e-2) << err_msg;
80  }
81 }
82 
83 TEST(Path2D, Curvature)
84 {
85  for (float c = 1.0; c < 4.0; c += 0.4)
86  {
87  trajectory_tracker::Path2D path;
88  for (double a = 0; a < 1.57; a += 0.1)
89  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(std::cos(a), std::sin(a)) * c, a + M_PI / 2, 1));
90  ASSERT_NEAR(path.getCurvature(path.begin(), path.end(), path[0].pos_, 10.0), 1.0 / c, 1e-2);
91  }
92 
93  for (float c = 1.0; c < 4.0; c += 0.4)
94  {
95  trajectory_tracker::Path2D path;
96  for (double a = 0; a < 0.2; a += 0.1)
97  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(std::cos(a), std::sin(a)) * c, a + M_PI / 2, 1));
98  ASSERT_NEAR(path.getCurvature(path.begin(), path.end(), path[0].pos_, 10.0), 1.0 / c, 1e-2);
99  }
100 }
101 
102 TEST(Path2D, LocalGoalWithoutSwitchBack)
103 {
104  for (int yaw_i = -1; yaw_i <= 1; ++yaw_i)
105  {
106  const double yaw_diff = yaw_i * 0.1;
107 
108  trajectory_tracker::Path2D path;
109  Eigen::Vector2d p(0, 0);
110  double yaw(0);
111  for (int i = 0; i < 10; ++i)
112  {
113  p -= Eigen::Vector2d(std::cos(yaw), std::sin(yaw)) * 0.1;
114  yaw += yaw_diff;
115  path.push_back(trajectory_tracker::Pose2D(p, yaw, 1));
116  }
117  ASSERT_EQ(path.findLocalGoal(path.begin(), path.end(), true), path.end());
118  ASSERT_EQ(path.findLocalGoal(path.begin(), path.end(), false), path.end());
119  }
120 }
121 
122 TEST(Path2D, LocalGoalWithSwitchBack)
123 {
124  for (int yaw_i = -1; yaw_i <= 1; ++yaw_i)
125  {
126  const double yaw_diff = yaw_i * 0.1;
127 
128  trajectory_tracker::Path2D path;
129  Eigen::Vector2d p(0, 0);
130  double yaw(0);
131  for (int i = 0; i < 5; ++i)
132  {
133  p -= Eigen::Vector2d(std::cos(yaw), std::sin(yaw)) * 0.1;
134  yaw += yaw_diff;
135  path.push_back(trajectory_tracker::Pose2D(p, yaw, 1));
136  }
137  for (int i = 0; i < 5; ++i)
138  {
139  p += Eigen::Vector2d(std::cos(yaw), std::sin(yaw)) * 0.1;
140  yaw += yaw_diff;
141  path.push_back(trajectory_tracker::Pose2D(p, yaw, 1));
142  }
143 
144  {
145  const auto it_local_goal = path.findLocalGoal(path.begin(), path.end(), true, true);
146  ASSERT_EQ(it_local_goal, path.begin() + 5);
147  ASSERT_EQ(path.findLocalGoal(it_local_goal, path.end(), true, true), path.end());
148  }
149  {
150  const auto it_local_goal = path.findLocalGoal(path.begin(), path.end(), true, false);
151  ASSERT_EQ(it_local_goal, path.begin() + 5);
152  ASSERT_EQ(path.findLocalGoal(it_local_goal, path.end(), true, false), path.end());
153  }
154  // no switch back motion under (allow_switchback == false)
155  ASSERT_EQ(path.findLocalGoal(path.begin(), path.end(), false, true), path.end());
156  }
157 }
158 
159 TEST(Path2D, LocalGoalTest)
160 {
161  trajectory_tracker::Path2D path;
162  Eigen::Vector2d p(0, 0);
163  for (size_t i = 0; i <= 10; ++i)
164  {
165  const double yaw = M_PI / 2 / 10;
166  p.x() = std::sin(i * yaw);
167  p.y() = 1.0 - std::cos(i * yaw);
168  path.push_back(trajectory_tracker::Pose2D(p, yaw, 0));
169  }
170  // Turn in-place and go back
171  for (size_t i = 0; i <= 10; ++i)
172  {
173  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(1.0 - i * 0.1, 1.0), 0, 0));
174  }
175  {
176  const auto it_local_goal = path.findLocalGoal(path.begin(), path.end(), true, true);
177  ASSERT_EQ(it_local_goal, path.begin() + 11);
178  ASSERT_EQ(path.findLocalGoal(it_local_goal, path.end(), true, true), path.end());
179  }
180  {
181  ASSERT_EQ(path.findLocalGoal(path.begin(), path.end(), true, false), path.end());
182  }
183 }
184 
185 TEST(Path2D, EnumerateLocalGoals)
186 {
187  trajectory_tracker::Path2D path;
188  const std::vector<float> yaws = {0.5, -0.2, 1.0};
189  double x = 0.0;
190  double y = 0.0;
191  for (const float yaw : yaws)
192  {
193  for (size_t i = 0; i < 10; ++i)
194  {
195  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(x, y), yaw, 1.0));
196  x += std::cos(yaw) * 0.1;
197  y += std::sin(yaw) * 0.1;
198  }
199  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(x, y), yaw, 1.0));
200  }
201  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(x, y), 0.3, 1.0));
202 
203  const auto local_goals = path.enumerateLocalGoals(path.begin(), path.end(), true);
204  ASSERT_EQ(local_goals.size(), 3);
205  ASSERT_EQ(local_goals.at(0) - path.begin(), 11);
206  ASSERT_EQ(local_goals.at(1) - path.begin(), 22);
207  ASSERT_EQ(local_goals.at(2) - path.begin(), 33);
208 }
209 
210 TEST(Path2D, FindNearestWithDistance)
211 {
212  trajectory_tracker::Path2D path;
213  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.5, 0.5), 0, 1));
214  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.6, 0.5), 0, 1));
215  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.7, 0.5), 0, 1));
216  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.8, 0.6), M_PI / 4, 1));
217  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.7), M_PI / 4, 1));
218  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.8), M_PI / 2, 1));
219  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.9), M_PI / 2, 1));
220 
221  {
222  // The nearest line is (0.8, 0.6) - (0.9, 0.7), and the nearest point on the line is (0.85, 0.65).
223  const auto nearest_with_dist = path.findNearestWithDistance(path.begin(), path.end(), Eigen::Vector2d(1.0, 0.5));
224  EXPECT_EQ(nearest_with_dist.first, path.begin() + 4);
225  EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.85, 2) + std::pow(0.5 - 0.65, 2)), 1.0e-6);
226  }
227  {
228  // The nearest point is (0.7, 0.5).
229  const auto nearest_with_dist =
230  path.findNearestWithDistance(path.begin(), path.begin() + 3, Eigen::Vector2d(1.0, 0.5));
231  EXPECT_EQ(nearest_with_dist.first, path.begin() + 2);
232  EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.7, 2) + std::pow(0.5 - 0.5, 2)), 1.0e-6);
233  }
234  {
235  // Test edge cases
236  const auto nearest_with_dist =
237  path.findNearestWithDistance(path.begin() + 5, path.begin() + 5, Eigen::Vector2d(1.0, 0.5));
238  EXPECT_EQ(nearest_with_dist.first, path.begin() + 5);
239  EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.9, 2) + std::pow(0.5 - 0.8, 2)), 1.0e-6);
240 
241  const auto invalid_result =
242  path.findNearestWithDistance(path.end(), path.end(), Eigen::Vector2d(1.0, 0.5));
243  EXPECT_EQ(invalid_result.first, path.end());
244  EXPECT_EQ(invalid_result.second, std::numeric_limits<double>::max());
245  }
246 }
247 
248 TEST(Path2D, Conversions)
249 {
250  nav_msgs::Path path_msg_org;
251  path_msg_org.poses.resize(8);
252  path_msg_org.poses[0].pose.position.x = 0.0;
253  path_msg_org.poses[0].pose.position.y = 0.0;
254  path_msg_org.poses[0].pose.orientation.w = 1.0;
255  path_msg_org.poses[1].pose.position.x = 1.0; // Start of in-place turning
256  path_msg_org.poses[1].pose.position.y = 0.0;
257  path_msg_org.poses[1].pose.orientation.w = 1.0;
258  path_msg_org.poses[2].pose.position.x = 1.0;
259  path_msg_org.poses[2].pose.position.y = 0.0;
260  path_msg_org.poses[2].pose.orientation.z = std::sin(M_PI / 8);
261  path_msg_org.poses[2].pose.orientation.w = std::cos(M_PI / 8);
262  path_msg_org.poses[3].pose.position.x = 1.0; // End of in-place turning
263  path_msg_org.poses[3].pose.position.y = 0.0;
264  path_msg_org.poses[3].pose.orientation.z = std::sin(M_PI / 4);
265  path_msg_org.poses[3].pose.orientation.w = std::cos(M_PI / 4);
266  path_msg_org.poses[4].pose.position.x = 1.0;
267  path_msg_org.poses[4].pose.position.y = 1.0;
268  path_msg_org.poses[4].pose.orientation.z = std::sin(M_PI / 4);
269  path_msg_org.poses[4].pose.orientation.w = std::cos(M_PI / 4);
270  path_msg_org.poses[5].pose.position.x = 1.0; // Start of in-place turning
271  path_msg_org.poses[5].pose.position.y = 2.0;
272  path_msg_org.poses[5].pose.orientation.z = std::sin(M_PI / 4);
273  path_msg_org.poses[5].pose.orientation.w = std::cos(M_PI / 4);
274  path_msg_org.poses[6].pose.position.x = 1.0;
275  path_msg_org.poses[6].pose.position.y = 2.0;
276  path_msg_org.poses[6].pose.orientation.z = std::sin(M_PI / 4 + M_PI / 6);
277  path_msg_org.poses[6].pose.orientation.w = std::cos(M_PI / 4 + M_PI / 6);
278  path_msg_org.poses[7].pose.position.x = 1.0; // End of in-place turning
279  path_msg_org.poses[7].pose.position.y = 2.0;
280  path_msg_org.poses[7].pose.orientation.z = std::sin(M_PI / 4 + M_PI / 3);
281  path_msg_org.poses[7].pose.orientation.w = std::cos(M_PI / 4 + M_PI / 3);
282 
283  trajectory_tracker::Path2D path;
284  path.fromMsg(path_msg_org);
285 
286  const std::vector<size_t> expected_org_indexes = {0, 1, 3, 4, 5, 7};
287  ASSERT_EQ(path.size(), 6);
288  for (size_t i = 0; i < path.size(); ++i)
289  {
290  const size_t org_index = expected_org_indexes[i];
291  EXPECT_EQ(path[i].pos_.x(), path_msg_org.poses[org_index].pose.position.x) << "i: " << i << " org: " << org_index;
292  EXPECT_EQ(path[i].pos_.y(), path_msg_org.poses[org_index].pose.position.y) << "i: " << i << " org: " << org_index;
293  EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg_org.poses[org_index].pose.orientation), 1.0e-6)
294  << "i: " << i << " org: " << org_index;
295  EXPECT_TRUE(std::isnan(path[i].velocity_));
296  }
297 
298  nav_msgs::Path path_msg;
299  path_msg.header.frame_id = "map";
300  path_msg.header.stamp = ros::Time(123.456);
301  path.toMsg(path_msg);
302  ASSERT_EQ(path_msg.poses.size(), 6);
303  for (size_t i = 0; i < path.size(); ++i)
304  {
305  EXPECT_EQ(path[i].pos_.x(), path_msg.poses[i].pose.position.x) << "i: " << i;
306  EXPECT_EQ(path[i].pos_.y(), path_msg.poses[i].pose.position.y) << "i: " << i;
307  EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg.poses[i].pose.orientation), 1.0e-6) << "i: " << i;
308  EXPECT_EQ(path_msg.poses[i].header.frame_id, path_msg.header.frame_id);
309  EXPECT_EQ(path_msg.poses[i].header.stamp, path_msg.header.stamp);
310  }
311 
312  trajectory_tracker_msgs::PathWithVelocity path_with_vel_msg_org;
313  path_with_vel_msg_org.poses.resize(path_msg_org.poses.size());
314  for (size_t i = 0; i < path_msg_org.poses.size(); ++i)
315  {
316  path_with_vel_msg_org.poses[i].pose = path_msg_org.poses[i].pose;
317  path_with_vel_msg_org.poses[i].linear_velocity.x = i * 0.1;
318  }
319 
320  trajectory_tracker::Path2D path_with_vel;
321  path_with_vel.fromMsg(path_with_vel_msg_org);
322 
323  ASSERT_EQ(path_with_vel.size(), 6);
324  for (size_t i = 0; i < path_with_vel.size(); ++i)
325  {
326  const size_t org_index = expected_org_indexes[i];
327  EXPECT_EQ(path_with_vel[i].pos_.x(), path_with_vel_msg_org.poses[org_index].pose.position.x)
328  << "i: " << i << " org: " << org_index;
329  EXPECT_EQ(path_with_vel[i].pos_.y(), path_with_vel_msg_org.poses[org_index].pose.position.y)
330  << "i: " << i << " org: " << org_index;
331  EXPECT_NEAR(path_with_vel[i].yaw_, tf2::getYaw(path_with_vel_msg_org.poses[org_index].pose.orientation), 1.0e-6)
332  << "i: " << i << " org: " << org_index;
333  EXPECT_NEAR(path_with_vel[i].velocity_, org_index * 0.1, 1.0e-6) << "i: " << i << " org: " << org_index;
334  }
335 
336  trajectory_tracker_msgs::PathWithVelocity path_with_vel_msg;
337  path_with_vel_msg.header.frame_id = "map";
338  path_with_vel_msg.header.stamp = ros::Time(123.456);
339  path_with_vel.toMsg(path_with_vel_msg);
340  ASSERT_EQ(path_with_vel_msg.poses.size(), 6);
341  for (size_t i = 0; i < path_with_vel.size(); ++i)
342  {
343  EXPECT_EQ(path_with_vel[i].pos_.x(), path_with_vel_msg.poses[i].pose.position.x) << "i: " << i;
344  EXPECT_EQ(path_with_vel[i].pos_.y(), path_with_vel_msg.poses[i].pose.position.y) << "i: " << i;
345  EXPECT_NEAR(path_with_vel[i].yaw_, tf2::getYaw(path_with_vel_msg.poses[i].pose.orientation), 1.0e-6)
346  << "i: " << i;
347  EXPECT_EQ(path_with_vel_msg.poses[i].header.frame_id, path_with_vel_msg.header.frame_id);
348  EXPECT_EQ(path_with_vel_msg.poses[i].header.stamp, path_with_vel_msg.header.stamp);
349  }
350 }
351 
352 TEST(Path2D, EstimatedTimeOfArrivals)
353 {
354  trajectory_tracker::Path2D path;
355  // Straight
356  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.0, 0.0), 135.0 / 180.0 * M_PI, 0));
357  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.1, -0.1), 135.0 / 180.0 * M_PI, 0));
358  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.2, -0.2), 135.0 / 180.0 * M_PI, 0));
359  // In-place-turn
360  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.2, -0.2), -M_PI / 2, 0));
361  // Curve
362  for (int i = 1; i <= 4; ++i)
363  {
364  const double angle = M_PI / 8 * i;
365  path.push_back(trajectory_tracker::Pose2D(
366  Eigen::Vector2d(0.2 * std::cos(angle), -0.2 + 0.2 * std::sin(angle)), -M_PI / 2 - angle, 0));
367  }
368  const double linear_speed = 0.5;
369  const double angular_speed = M_PI;
370  const std::vector<double> etas =
371  path.getEstimatedTimeOfArrivals(path.begin(), path.end(), linear_speed, angular_speed, 1.0);
372  ASSERT_EQ(etas.size(), 8);
373  double expected_eta = 1.0;
374  EXPECT_NEAR(etas[0], expected_eta, 1.0e-6);
375  expected_eta += std::hypot(0.1, 0.1) / linear_speed;
376  EXPECT_NEAR(etas[1], expected_eta, 1.0e-6);
377  expected_eta += std::hypot(0.1, 0.1) / linear_speed;
378  EXPECT_NEAR(etas[2], expected_eta, 1.0e-6);
379  expected_eta += 135.0 / (180.0 / M_PI) / angular_speed;
380  EXPECT_NEAR(etas[3], expected_eta, 1.0e-6);
381  const double turn_dist = std::hypot(0.2 - 0.2 * std::cos(M_PI / 8), 0.2 * std::sin(M_PI / 8));
382  for (size_t p = 4; p < etas.size(); ++p)
383  {
384  EXPECT_NEAR(etas[p], expected_eta + (p - 3) * turn_dist / linear_speed, 1.0e-6);
385  }
386 }
387 
388 int main(int argc, char** argv)
389 {
390  testing::InitGoogleTest(&argc, argv);
391 
392  return RUN_ALL_TESTS();
393 }
trajectory_tracker::Path2D::remainedDistance
double remainedDistance(const ConstIterator &begin, const ConstIterator &nearest, const ConstIterator &end, const Eigen::Vector2d &target_on_line) const
Definition: path2d.h:226
tf2::getYaw
double getYaw(const A &a)
trajectory_tracker::Pose2D
Definition: path2d.h:52
trajectory_tracker::Path2D
Definition: path2d.h:118
TEST
TEST(Path2D, RemainedDistance)
Definition: test_path2d.cpp:55
trajectory_tracker::Path2D::findNearestWithDistance
std::pair< ConstIterator, double > findNearestWithDistance(const ConstIterator &begin, const ConstIterator &end, const Eigen::Vector2d &target, const float max_search_range=0, const float epsilon=1e-6) const
Definition: path2d.h:180
trajectory_tracker::Path2D::findLocalGoal
ConstIterator findLocalGoal(const ConstIterator &begin, const ConstIterator &end, const bool allow_switch_back, const bool allow_in_place_turn=true, const double epsilon=1e-6) const
Definition: path2d.h:134
trajectory_tracker::Path2D::toMsg
void toMsg(PATH_TYPE &path) const
Definition: path2d.h:342
trajectory_tracker::Path2D::fromMsg
void fromMsg(const PATH_TYPE &path, const double in_place_turn_eps=1.0e-6)
Definition: path2d.h:306
trajectory_tracker::projection2d
Eigen::Vector2d projection2d(const Eigen::Vector2d &a, const Eigen::Vector2d &b, const Eigen::Vector2d &c)
Definition: eigen_line.h:89
main
int main(int argc, char **argv)
Definition: test_path2d.cpp:388
eigen_line.h
trajectory_tracker::Path2D::enumerateLocalGoals
std::vector< ConstIterator > enumerateLocalGoals(const ConstIterator &begin, const ConstIterator &end, const bool allow_switch_back, const bool allow_in_place_turn=true, const double epsilon=1e-6) const
Definition: path2d.h:353
ros::Time
angle
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
trajectory_tracker::Path2D::getCurvature
float getCurvature(const ConstIterator &begin, const ConstIterator &end, const Eigen::Vector2d &target_on_line, const float max_search_range) const
Definition: path2d.h:260
path2d.h
trajectory_tracker::Path2D::findNearest
ConstIterator findNearest(const ConstIterator &begin, const ConstIterator &end, const Eigen::Vector2d &target, const float max_search_range=0, const float epsilon=1e-6) const
Definition: path2d.h:171
trajectory_tracker::Path2D::getEstimatedTimeOfArrivals
std::vector< double > getEstimatedTimeOfArrivals(const ConstIterator &begin, const ConstIterator &end, const double linear_speed, const double angular_speed, const double initial_eta_sec=0.0) const
Definition: path2d.h:374

trajectory_tracker
Author(s): Atsushi Watanabe
autogenerated on Wed Mar 19 2025 02:14:15