test_planner_3d_cost.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <list>
31 #include <vector>
32 
33 #include <costmap_cspace_msgs/MapMetaData3D.h>
35 
36 #include <gtest/gtest.h>
37 
38 namespace planner_cspace
39 {
40 namespace planner_3d
41 {
43 {
44  costmap_cspace_msgs::MapMetaData3D map_info;
45  map_info.width = 100;
46  map_info.height = 100;
47  map_info.angle = 16;
48  map_info.linear_resolution = 1.0;
49  map_info.angular_resolution = M_PI * 2 / map_info.angle;
51  BlockMemGridmap<float, 3, 2> cost_estim_cache(GridAstarModel3D::Vec(100, 100, 16));
52  cm.clear(0);
53  cost_estim_cache.clear(0.0);
54  CostCoeff cc;
55  cc.weight_decel_ = 0.1;
56  cc.weight_backward_ = 0.1;
57  cc.weight_ang_vel_ = 1.0;
58  cc.weight_costmap_ = 0.1;
59  cc.weight_costmap_turn_ = 0.1;
60  cc.weight_remembered_ = 0.0;
61  cc.weight_hysteresis_ = 0.0;
62  cc.in_place_turn_ = 0.0;
63  cc.hysteresis_max_dist_ = 0.0;
64  cc.hysteresis_expand_ = 0.0;
65  cc.min_curve_radius_ = 0.0;
66  cc.max_vel_ = 1.0;
67  cc.max_ang_vel_ = 1.0;
68  cc.angle_resolution_aspect_ = 1.0;
69 
70  GridAstarModel3D model(
71  map_info,
72  GridAstarModel3D::Vecf(1.0f, 1.0f, 0.1f),
73  100.0,
74  cost_estim_cache, cm, cm, cm,
75  cc, 10);
76 
77  const GridAstarModel3D::Vec start(54, 50, 8);
78  const GridAstarModel3D::Vec goal_straight(50, 50, 8);
79  const GridAstarModel3D::Vec goal_curve(50, 50, 7);
80  const GridAstarModel3D::Vec goal_drift(50, 51, 8);
81  const GridAstarModel3D::Vec goal_curve_drift(50, 51, 7);
82  const GridAstarModel3D::Vec goal_curve_sign_err(50, 51, 9);
83  std::vector<GridAstarModel3D::VecWithCost> starts;
84  starts.emplace_back(start);
85 
86  const float c_straight = model.cost(start, goal_straight, starts, goal_straight);
87  const float c_curve = model.cost(start, goal_curve, starts, goal_curve);
88  const float c_drift = model.cost(start, goal_drift, starts, goal_drift);
89  const float c_drift_curve = model.cost(start, goal_curve_drift, starts, goal_curve_drift);
90 
91  EXPECT_LT(c_straight, c_curve);
92  EXPECT_LT(c_straight, c_drift);
93  EXPECT_LT(c_straight, c_drift_curve);
94 }
95 } // namespace planner_3d
96 } // namespace planner_cspace
97 
98 int main(int argc, char** argv)
99 {
100  testing::InitGoogleTest(&argc, argv);
101 
102  return RUN_ALL_TESTS();
103 }
int main(int argc, char **argv)
f
TEST(CostmapBBF, ForEach)
float cost(const Vec &cur, const Vec &next, const std::vector< VecWithCost > &start, const Vec &goal) const override


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:42