35 #include <gtest/gtest.h>
43 dwb_msgs::Trajectory2D traj;
45 traj.time_offsets.resize(4);
46 for (
unsigned int i=0; i < traj.poses.size(); i++)
48 double d =
static_cast<double>(i);
67 dwb_msgs::Trajectory2D traj;
69 traj.time_offsets.resize(4);
70 for (
unsigned int i=0; i < traj.poses.size(); i++)
72 double d =
static_cast<double>(i);
74 traj.poses[i].y = 30.0 - 2.0 *
d;
75 traj.poses[i].theta = 0.42;
81 EXPECT_DOUBLE_EQ(
projectPose(traj, 0.0).theta, 0.42);
84 EXPECT_DOUBLE_EQ(
projectPose(traj, -1.0).theta, 0.42);
87 EXPECT_DOUBLE_EQ(
projectPose(traj, 0.4).theta, 0.42);
90 EXPECT_DOUBLE_EQ(
projectPose(traj, 0.5).theta, 0.42);
93 EXPECT_DOUBLE_EQ(
projectPose(traj, 0.51).theta, 0.42);
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);
102 EXPECT_DOUBLE_EQ(
projectPose(traj, 2.0).theta, 0.42);
105 EXPECT_DOUBLE_EQ(
projectPose(traj, 2.51).theta, 0.42);
108 EXPECT_DOUBLE_EQ(
projectPose(traj, 3.5).theta, 0.42);
111 int main(
int argc,
char **argv)
113 testing::InitGoogleTest(&argc, argv);
114 return RUN_ALL_TESTS();