00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <cstddef>
00031
00032 #include <gtest/gtest.h>
00033
00034 #include <planner_cspace/cyclic_vec.h>
00035 #include <planner_cspace/blockmem_gridmap.h>
00036 #include <planner_cspace/planner_3d/motion_cache.h>
00037
00038 TEST(MotionCache, Generate)
00039 {
00040 const int range = 4;
00041 const int angle = 4;
00042 const float angular_resolution = M_PI * 2 / angle;
00043 const float linear_resolution = 0.5;
00044
00045 BlockMemGridmap<char, 3, 2, 0x20> gm;
00046 MotionCache cache;
00047 cache.reset(
00048 linear_resolution, angular_resolution, range,
00049 gm.getAddressor());
00050
00051
00052 const int xy_yaw_straight[][3] =
00053 {
00054 { 1, 0, 0 },
00055 { 0, 1, 1 },
00056 { -1, 0, 2 },
00057 { 0, -1, 3 },
00058 };
00059 for (auto& xy_yaw : xy_yaw_straight)
00060 {
00061 for (int i = 1; i <= range + 1; ++i)
00062 {
00063 const CyclicVecInt<3, 2> goal(
00064 i * xy_yaw[0], i * xy_yaw[1], xy_yaw[2]);
00065 const auto c = cache.find(xy_yaw[2], goal);
00066 if (i > range)
00067 {
00068 ASSERT_EQ(c, cache.end(xy_yaw[2]));
00069 continue;
00070 }
00071 ASSERT_NE(c, cache.end(xy_yaw[2]));
00072 for (const auto& p : c->second.getMotion())
00073 {
00074
00075 if (xy_yaw[0] == 0)
00076 ASSERT_EQ(xy_yaw[0], 0);
00077 else
00078 ASSERT_GE(p[0] * xy_yaw[0], 0);
00079 if (xy_yaw[1] == 0)
00080 ASSERT_EQ(xy_yaw[1], 0);
00081 else
00082 ASSERT_GE(p[1] * xy_yaw[1], 0);
00083 }
00084 ASSERT_EQ(c->second.getDistance(), i);
00085 ASSERT_EQ(static_cast<int>(c->second.getMotion().size()), i);
00086 }
00087 }
00088
00089
00090 const int xy_syaw_gyaw_90[][4] =
00091 {
00092 { 1, 1, 0, 1 },
00093 { -1, 1, 1, 2 },
00094 { -1, -1, 2, 3 },
00095 { 1, -1, 3, 0 },
00096 { 1, -1, 0, 3 },
00097 { 1, 1, 1, 0 },
00098 { -1, 1, 2, 1 },
00099 { -1, -1, 3, 2 },
00100 };
00101 for (auto& xy_syaw_gyaw : xy_syaw_gyaw_90)
00102 {
00103 for (int i = 1; i <= range + 1; ++i)
00104 {
00105 const CyclicVecInt<3, 2> goal(
00106 i * xy_syaw_gyaw[0], i * xy_syaw_gyaw[1], xy_syaw_gyaw[3]);
00107 const auto c = cache.find(xy_syaw_gyaw[2], goal);
00108 if (i * sqrt(2) >= range)
00109 {
00110 ASSERT_EQ(c, cache.end(xy_syaw_gyaw[2]));
00111 continue;
00112 }
00113 ASSERT_NE(c, cache.end(xy_syaw_gyaw[2]));
00114
00115 for (const auto& p : c->second.getMotion())
00116 {
00117
00118 ASSERT_GE(p[0] * xy_syaw_gyaw[0], 0);
00119 ASSERT_GE(p[1] * xy_syaw_gyaw[1], 0);
00120 }
00121
00122 const float arc_length = i * 2 * M_PI / 4;
00123 EXPECT_NEAR(c->second.getDistance(), arc_length, 0.1);
00124 }
00125 }
00126 }
00127
00128 int main(int argc, char** argv)
00129 {
00130 testing::InitGoogleTest(&argc, argv);
00131
00132 return RUN_ALL_TESTS();
00133 }