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 <costmap_cspace_msgs/MapMetaData3D.h>
00031 #include <planner_cspace/planner_3d/grid_metric_converter.h>
00032 #include <tf2/utils.h>
00033
00034 #include <gtest/gtest.h>
00035
00036 #define _USE_MATH_DEFINES
00037 #include <cmath>
00038 #include <vector>
00039
00040 TEST(GridMetricConverter, SinglePose)
00041 {
00042 costmap_cspace_msgs::MapMetaData3D map_info;
00043 map_info.linear_resolution = 0.1;
00044 map_info.angular_resolution = M_PI / 8;
00045 map_info.origin.position.x = 100.0;
00046 map_info.origin.position.y = 100.0;
00047
00048 const float metric[][3] =
00049 {
00050 { 100.01, 100.01, M_PI / 8 },
00051 { 100.01, 100.01, -M_PI / 8 },
00052 { 105.01, 102.09, M_PI / 2 },
00053 { 100.21, 102.51, -M_PI / 2 },
00054 { 99.99, 100.01, 0 },
00055 { 100.01, 100.01, 2.1 * M_PI / 8 },
00056 { 100.01, 100.01, 1.9 * M_PI / 8 }
00057 };
00058 const int grid[][3] =
00059 {
00060 { 0, 0, 1 },
00061 { 0, 0, -1 },
00062 { 50, 20, 4 },
00063 { 2, 25, -4 },
00064 { -1, 0, 0 },
00065 { 0, 0, 2 },
00066 { 0, 0, 2 }
00067 };
00068 for (size_t i = 0; i < sizeof(grid) / sizeof(grid[0]); ++i)
00069 {
00070
00071 int output_grid[3];
00072 grid_metric_converter::metric2Grid(
00073 map_info,
00074 output_grid[0], output_grid[1], output_grid[2],
00075 metric[i][0], metric[i][1], metric[i][2]);
00076 for (size_t j = 0; j < 3; ++j)
00077 ASSERT_EQ(output_grid[j], grid[i][j]);
00078
00079
00080 float output_metric[3];
00081 grid_metric_converter::grid2Metric(
00082 map_info,
00083 grid[i][0], grid[i][1], grid[i][2],
00084 output_metric[0], output_metric[1], output_metric[2]);
00085 for (size_t j = 0; j < 3; ++j)
00086 ASSERT_NEAR(output_metric[j], metric[i][j], map_info.linear_resolution / 2);
00087 }
00088 }
00089
00090 TEST(GridMetricConverter, Path)
00091 {
00092 costmap_cspace_msgs::MapMetaData3D map_info;
00093 map_info.linear_resolution = 0.1;
00094 map_info.angle = 8;
00095 map_info.angular_resolution = M_PI / map_info.angle;
00096 map_info.origin.position.x = 100.0;
00097 map_info.origin.position.y = 100.0;
00098
00099 const float metric_expected[][3] =
00100 {
00101 { 100.15, 100.15, M_PI / 8 },
00102 { 100.15, 100.25, M_PI / 8 },
00103 { 100.25, 100.35, M_PI / 8 },
00104 { 100.35, 100.35, 2 * M_PI / 8 }
00105 };
00106 const int grid[][3] =
00107 {
00108 { 1, 1, 1 },
00109 { 1, 2, 1 },
00110 { 2, 3, 1 },
00111 { 3, 3, 2 }
00112 };
00113 std::vector<CyclicVecFloat<3, 2>> path_grid;
00114 for (const auto& g : grid)
00115 path_grid.push_back(CyclicVecFloat<3, 2>(g[0], g[1], g[2]));
00116
00117 nav_msgs::Path path;
00118 grid_metric_converter::grid2MetricPath(map_info, path_grid, path);
00119 size_t ref = 0;
00120 for (const geometry_msgs::PoseStamped& p : path.poses)
00121 {
00122 const double diff_dist = hypotf(
00123 metric_expected[ref][0] - p.pose.position.x,
00124 metric_expected[ref][1] - p.pose.position.y);
00125 double diff_yaw = fabs(tf2::getYaw(p.pose.orientation) - metric_expected[ref][2]);
00126
00127 if (diff_yaw < -M_PI)
00128 diff_yaw += 2.0 * M_PI;
00129 else if (diff_yaw > M_PI)
00130 diff_yaw -= 2.0 * M_PI;
00131
00132 ASSERT_LT(diff_dist, map_info.linear_resolution * 2);
00133 ASSERT_LT(fabs(diff_yaw), map_info.angular_resolution * 2);
00134
00135 if (diff_dist < map_info.linear_resolution / 2 &&
00136 fabs(diff_yaw) < map_info.angular_resolution / 2)
00137 {
00138 ++ref;
00139 if (ref == sizeof(metric_expected) / sizeof(metric_expected[0]))
00140 return;
00141 }
00142 }
00143
00144 ASSERT_TRUE(false);
00145 }
00146
00147 int main(int argc, char** argv)
00148 {
00149 testing::InitGoogleTest(&argc, argv);
00150
00151 return RUN_ALL_TESTS();
00152 }