utils_test.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, 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 #include <gtest/gtest.h>
37 
40 
41 TEST(Utils, ClosestPose)
42 {
43  dwb_msgs::Trajectory2D traj;
44  traj.poses.resize(4);
45  traj.time_offsets.resize(4);
46  for (unsigned int i=0; i < traj.poses.size(); i++)
47  {
48  double d = static_cast<double>(i);
49  traj.poses[i].x = d;
50  traj.time_offsets[i] = ros::Duration(d);
51  }
52 
53  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.0).x, traj.poses[0].x);
54  EXPECT_DOUBLE_EQ(getClosestPose(traj, -1.0).x, traj.poses[0].x);
55  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.4).x, traj.poses[0].x);
56  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.5).x, traj.poses[0].x);
57  EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.51).x, traj.poses[1].x);
58  EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.0).x, traj.poses[1].x);
59  EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.4999).x, traj.poses[1].x);
60  EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.0).x, traj.poses[2].x);
61  EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.51).x, traj.poses[3].x);
62  EXPECT_DOUBLE_EQ(getClosestPose(traj, 3.5).x, traj.poses[3].x);
63 }
64 
65 TEST(Utils, ProjectPose)
66 {
67  dwb_msgs::Trajectory2D traj;
68  traj.poses.resize(4);
69  traj.time_offsets.resize(4);
70  for (unsigned int i=0; i < traj.poses.size(); i++)
71  {
72  double d = static_cast<double>(i);
73  traj.poses[i].x = d;
74  traj.poses[i].y = 30.0 - 2.0 * d;
75  traj.poses[i].theta = 0.42;
76  traj.time_offsets[i] = ros::Duration(d);
77  }
78 
79  EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).x, 0.0);
80  EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).y, 30.0);
81  EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).theta, 0.42);
82  EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).x, 0.0);
83  EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).y, 30.0);
84  EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).theta, 0.42);
85  EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).x, 0.4);
86  EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).y, 29.2);
87  EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).theta, 0.42);
88  EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).x, 0.5);
89  EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).y, 29.0);
90  EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).theta, 0.42);
91  EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).x, 0.51);
92  EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).y, 28.98);
93  EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).theta, 0.42);
94  EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).x, 1.0);
95  EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).y, 28.0);
96  EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).theta, 0.42);
97  EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).x, 1.4999);
98  EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).y, 27.0002);
99  EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).theta, 0.42);
100  EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).x, 2.0);
101  EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).y, 26.0);
102  EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).theta, 0.42);
103  EXPECT_FLOAT_EQ(projectPose(traj, 2.51).x, 2.51);
104  EXPECT_FLOAT_EQ(projectPose(traj, 2.51).y, 24.98);
105  EXPECT_DOUBLE_EQ(projectPose(traj, 2.51).theta, 0.42);
106  EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).x, 3.0);
107  EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).y, 24.0);
108  EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).theta, 0.42);
109 }
110 
111 int main(int argc, char **argv)
112 {
113  testing::InitGoogleTest(&argc, argv);
114  return RUN_ALL_TESTS();
115 }
d
TEST(Utils, ClosestPose)
Definition: utils_test.cpp:41
const geometry_msgs::Pose2D & getClosestPose(const dwb_msgs::Trajectory2D &trajectory, const double time_offset)
Helper function to find a pose in the trajectory with a particular time time_offset.
geometry_msgs::Pose2D projectPose(const dwb_msgs::Trajectory2D &trajectory, const double time_offset)
Helper function to create a pose with an exact time_offset by linearly interpolating between existing...
TFSIMD_FORCE_INLINE const tfScalar & y() const
int main(int argc, char **argv)
Definition: utils_test.cpp:111
TFSIMD_FORCE_INLINE const tfScalar & x() const


dwb_local_planner
Author(s): David V. Lu!!
autogenerated on Wed Jun 26 2019 20:06:13