map_grid_cost_function.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: TKruse
36  *********************************************************************/
37 
38 #ifndef MAP_GRID_COST_FUNCTION_H_
39 #define MAP_GRID_COST_FUNCTION_H_
40 
42 
43 #include <costmap_2d/costmap_2d.h>
45 
46 namespace base_local_planner {
47 
54 
73 class MapGridCostFunction: public base_local_planner::TrajectoryCostFunction {
74 public:
76  double xshift = 0.0,
77  double yshift = 0.0,
78  bool is_local_goal_function = false,
79  CostAggregationType aggregationType = Last);
80 
82 
86  void setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses);
87 
88  void setXShift(double xshift) {xshift_ = xshift;}
89  void setYShift(double yshift) {yshift_ = yshift;}
90 
94  void setStopOnFailure(bool stop_on_failure) {stop_on_failure_ = stop_on_failure;}
95 
99  bool prepare();
100 
101  double scoreTrajectory(Trajectory &traj);
102 
106  double obstacleCosts() {
107  return map_.obstacleCosts();
108  }
109 
114  double unreachableCellCosts() {
115  return map_.unreachableCellCosts();
116  }
117 
118  // used for easier debugging
119  double getCellCosts(unsigned int cx, unsigned int cy);
120 
121 private:
122  std::vector<geometry_msgs::PoseStamped> target_poses_;
124 
128  // ooints of robots than center, like fron or back
129  // this can help with alignment or keeping specific
130  // wheels on tracks both default to 0
131  double xshift_;
132  double yshift_;
133  // if true, we look for a suitable local goal on path, else we use the full path for costs
135  bool stop_on_failure_;
136 };
137 
138 } /* namespace base_local_planner */
139 #endif /* MAP_GRID_COST_FUNCTION_H_ */
base_local_planner::Sum
@ Sum
Definition: map_grid_cost_function.h:123
base_local_planner::MapGridCostFunction::getCellCosts
double getCellCosts(unsigned int cx, unsigned int cy)
Definition: map_grid_cost_function.cpp:105
base_local_planner::MapGrid::unreachableCellCosts
double unreachableCellCosts()
Definition: map_grid.h:176
base_local_planner::MapGridCostFunction::stop_on_failure_
bool stop_on_failure_
Definition: map_grid_cost_function.h:170
base_local_planner::MapGridCostFunction::target_poses_
std::vector< geometry_msgs::PoseStamped > target_poses_
Definition: map_grid_cost_function.h:157
base_local_planner::MapGridCostFunction::yshift_
double yshift_
Definition: map_grid_cost_function.h:167
costmap_2d.h
base_local_planner::MapGridCostFunction::obstacleCosts
double obstacleCosts()
Definition: map_grid_cost_function.h:141
costmap_2d::Costmap2D
base_local_planner::MapGridCostFunction::unreachableCellCosts
double unreachableCellCosts()
Definition: map_grid_cost_function.h:149
base_local_planner::CostAggregationType
CostAggregationType
Definition: map_grid_cost_function.h:88
base_local_planner::MapGridCostFunction::costmap_
costmap_2d::Costmap2D * costmap_
Definition: map_grid_cost_function.h:158
base_local_planner::MapGridCostFunction::scoreTrajectory
double scoreTrajectory(Trajectory &traj)
Definition: map_grid_cost_function.cpp:110
base_local_planner::MapGridCostFunction::prepare
bool prepare()
Definition: map_grid_cost_function.cpp:94
base_local_planner::TrajectoryCostFunction
Provides an interface for critics of trajectories During each sampling run, a batch of many trajector...
Definition: trajectory_cost_function.h:87
map_grid.h
base_local_planner::MapGridCostFunction::setTargetPoses
void setTargetPoses(std::vector< geometry_msgs::PoseStamped > target_poses)
Definition: map_grid_cost_function.cpp:90
base_local_planner::MapGridCostFunction::aggregationType_
CostAggregationType aggregationType_
Definition: map_grid_cost_function.h:161
base_local_planner::MapGrid::obstacleCosts
double obstacleCosts()
Definition: map_grid.h:168
base_local_planner::MapGridCostFunction::MapGridCostFunction
MapGridCostFunction(costmap_2d::Costmap2D *costmap, double xshift=0.0, double yshift=0.0, bool is_local_goal_function=false, CostAggregationType aggregationType=Last)
Definition: map_grid_cost_function.cpp:77
base_local_planner::Last
@ Last
Definition: map_grid_cost_function.h:123
base_local_planner::Product
@ Product
Definition: map_grid_cost_function.h:123
base_local_planner::MapGrid
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
Definition: map_grid.h:84
base_local_planner::MapGridCostFunction::is_local_goal_function_
bool is_local_goal_function_
Definition: map_grid_cost_function.h:169
base_local_planner::MapGridCostFunction::map_
base_local_planner::MapGrid map_
Definition: map_grid_cost_function.h:160
base_local_planner::MapGridCostFunction::setStopOnFailure
void setStopOnFailure(bool stop_on_failure)
If true, failures along the path cause the entire path to be rejected.
Definition: map_grid_cost_function.h:129
base_local_planner::MapGridCostFunction::~MapGridCostFunction
~MapGridCostFunction()
Definition: map_grid_cost_function.h:116
base_local_planner
Definition: costmap_model.h:44
trajectory_cost_function.h
base_local_planner::MapGridCostFunction::xshift_
double xshift_
xshift and yshift allow scoring for different
Definition: map_grid_cost_function.h:166
base_local_planner::MapGridCostFunction::setXShift
void setXShift(double xshift)
Definition: map_grid_cost_function.h:123
base_local_planner::MapGridCostFunction::setYShift
void setYShift(double yshift)
Definition: map_grid_cost_function.h:124


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:24