planner_3d/grid_astar_model.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019-2020, 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 #ifndef PLANNER_CSPACE_PLANNER_3D_GRID_ASTAR_MODEL_H
31 #define PLANNER_CSPACE_PLANNER_3D_GRID_ASTAR_MODEL_H
32 
33 #include <memory>
34 #include <utility>
35 #include <vector>
36 
37 #include <costmap_cspace_msgs/MapMetaData3D.h>
38 
45 
46 namespace planner_cspace
47 {
48 namespace planner_3d
49 {
50 class CostCoeff
51 {
52 public:
64  float max_vel_;
65  float max_ang_vel_;
67 };
68 
70 {
71 public:
72  friend class GridAstarModel2D;
73  using Ptr = std::shared_ptr<GridAstarModel3D>;
74  using ConstPtr = std::shared_ptr<const GridAstarModel3D>;
77 
79 
80 protected:
82  costmap_cspace_msgs::MapMetaData3D map_info_;
85  std::vector<std::vector<Vec>> motion_primitives_;
86  std::vector<Vec> search_list_rough_;
92  const CostCoeff& cc_;
93  int range_;
99  std::array<float, 1024> euclid_cost_lin_cache_;
100 
101 public:
102  explicit GridAstarModel3D(
103  const costmap_cspace_msgs::MapMetaData3D& map_info,
104  const Vecf& euclid_cost_coef,
105  const int local_range,
106  BlockMemGridmapBase<float, 3, 2>& cost_estim_cache,
110  const CostCoeff& cc,
111  const int range);
112  void enableHysteresis(const bool enable);
113  void createEuclidCostCache();
114  float euclidCost(const Vec& v) const;
115  float euclidCostRough(const Vec& v) const;
116  float cost(
117  const Vec& cur, const Vec& next, const std::vector<VecWithCost>& start, const Vec& goal) const override;
118 
119  float costEstim(
120  const Vec& cur, const Vec& goal) const override;
121  const std::vector<Vec>& searchGrids(
122  const Vec& p,
123  const std::vector<VecWithCost>& ss,
124  const Vec& es) const override;
125 };
126 
128 {
129 public:
130  using Ptr = std::shared_ptr<GridAstarModel2D>;
132 
133  inline explicit GridAstarModel2D(const GridAstarModel3D::ConstPtr base)
134  : base_(base)
135  {
136  }
137 
138  float cost(
139  const Vec& cur, const Vec& next, const std::vector<VecWithCost>& start, const Vec& goal) const final;
140  float costEstim(
141  const Vec& cur, const Vec& goal) const final;
142  const std::vector<Vec>& searchGrids(
143  const Vec& cur, const std::vector<VecWithCost>& start, const Vec& goal) const final;
144 };
145 } // namespace planner_3d
146 } // namespace planner_cspace
147 
148 #endif // PLANNER_CSPACE_PLANNER_3D_GRID_ASTAR_MODEL_H
std::vector< std::vector< Vec > > motion_primitives_
BlockMemGridmapBase< float, 3, 2 > & cost_estim_cache_
std::shared_ptr< const GridAstarModel3D > ConstPtr
GridAstarModel2D(const GridAstarModel3D::ConstPtr base)


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