MRPT  1.9.9
nav_plan_geometry_utils_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
12 #include <cmath>
13 
14 TEST(NavTests, NavGeomUtils_collision_straight_circ_robot)
15 {
16  using namespace mrpt;
17  using namespace mrpt::math;
18  using namespace mrpt::nav;
19 
20  {
21  TPoint2D p0(0, 0), p1(1, 1);
22  double colDist;
23  {
24  const double R = 0.1;
25  TPoint2D pObs(1.0, 0.0);
27  p0, p1, R, pObs, colDist);
28  EXPECT_FALSE(ret);
29  }
30  {
31  const double R = 0.1;
32  TPoint2D pObs(0.0, 1.0);
34  p0, p1, R, pObs, colDist);
35  EXPECT_FALSE(ret);
36  }
37  {
38  const double R = 0.1;
39  TPoint2D pObs(-0.4, -0.4);
41  p0, p1, R, pObs, colDist);
42  EXPECT_FALSE(ret);
43  }
44  {
45  const double R = 0.1;
46  TPoint2D pObs(1.4, 1.4);
48  p0, p1, R, pObs, colDist);
49  EXPECT_FALSE(ret);
50  }
51  {
52  const double R = 0.1;
53  TPoint2D pObs(0.45, 0.48);
55  p0, p1, R, pObs, colDist);
56  EXPECT_TRUE(ret);
57  }
58  }
59 
60  {
61  TPoint2D p0(4, 5), p1(5, 5);
62  double colDist;
63  const double R = 0.5;
64  {
65  TPoint2D pObs(5.0, 5.0);
67  p0, p1, R, pObs, colDist);
68  EXPECT_TRUE(ret);
69  EXPECT_NEAR(colDist, 0.5, 1e-6);
70  }
71  {
72  TPoint2D pObs(4.510, 5.001);
74  p0, p1, R, pObs, colDist);
75  EXPECT_TRUE(ret);
76  EXPECT_NEAR(colDist, 0.01, 3e-3);
77  }
78  }
79  {
80  TPoint2D p0(0, 0), p1(-2, -1);
81  double colDist;
82  const double R = 0.5;
83  TPoint2D pObs(-2.0, -1.0);
84  const bool ret =
85  collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
86  EXPECT_TRUE(ret);
87  EXPECT_NEAR(colDist, std::sqrt(5.0) - 0.5, 1e-6);
88  }
89 }
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
This base provides a set of functions for maths stuff.
bool collision_free_dist_segment_circ_robot(const mrpt::math::TPoint2D &p_start, const mrpt::math::TPoint2D &p_end, const double robot_radius, const mrpt::math::TPoint2D &obstacle, double &out_col_dist)
Computes the collision-free distance for a linear segment path between two points, for a circular robot, and a point obstacle (ox,oy).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020