obstacle_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 OBSTACLE_COST_FUNCTION_H_
39 #define OBSTACLE_COST_FUNCTION_H_
40 
42 
44 #include <costmap_2d/costmap_2d.h>
45 
46 namespace base_local_planner {
47 
53 class ObstacleCostFunction : public TrajectoryCostFunction {
54 
55 public:
58 
59  bool prepare();
60  double scoreTrajectory(Trajectory &traj);
61 
62  void setSumScores(bool score_sums){ sum_scores_=score_sums; }
63 
64  void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed);
65  void setFootprint(std::vector<geometry_msgs::Point> footprint_spec);
66 
67  // helper functions, made static for easy unit testing
68  static double getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor);
69  static double footprintCost(
70  const double& x,
71  const double& y,
72  const double& th,
73  double scale,
74  std::vector<geometry_msgs::Point> footprint_spec,
75  costmap_2d::Costmap2D* costmap,
76  base_local_planner::WorldModel* world_model);
77 
78 private:
80  std::vector<geometry_msgs::Point> footprint_spec_;
82  double max_trans_vel_;
83  bool sum_scores_;
84  //footprint scaling with velocity;
86 };
87 
88 } /* namespace base_local_planner */
89 #endif /* OBSTACLE_COST_FUNCTION_H_ */
base_local_planner::ObstacleCostFunction::max_trans_vel_
double max_trans_vel_
Definition: obstacle_cost_function.h:152
base_local_planner::ObstacleCostFunction::scoreTrajectory
double scoreTrajectory(Trajectory &traj)
Definition: obstacle_cost_function.cpp:109
base_local_planner::ObstacleCostFunction::setSumScores
void setSumScores(bool score_sums)
Definition: obstacle_cost_function.h:132
costmap_2d.h
costmap_2d::Costmap2D
base_local_planner::ObstacleCostFunction::costmap_
costmap_2d::Costmap2D * costmap_
Definition: obstacle_cost_function.h:149
base_local_planner::ObstacleCostFunction::setFootprint
void setFootprint(std::vector< geometry_msgs::Point > footprint_spec)
Definition: obstacle_cost_function.cpp:101
base_local_planner::ObstacleCostFunction::setParams
void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed)
Definition: obstacle_cost_function.cpp:94
base_local_planner::ObstacleCostFunction::footprint_spec_
std::vector< geometry_msgs::Point > footprint_spec_
Definition: obstacle_cost_function.h:150
base_local_planner::ObstacleCostFunction::sum_scores_
bool sum_scores_
Definition: obstacle_cost_function.h:153
costmap_model.h
base_local_planner::ObstacleCostFunction::~ObstacleCostFunction
~ObstacleCostFunction()
Definition: obstacle_cost_function.cpp:87
base_local_planner::ObstacleCostFunction::world_model_
base_local_planner::WorldModel * world_model_
Definition: obstacle_cost_function.h:151
base_local_planner::ObstacleCostFunction::scaling_speed_
double scaling_speed_
Definition: obstacle_cost_function.h:155
base_local_planner::WorldModel
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
Definition: world_model.h:87
base_local_planner::ObstacleCostFunction::ObstacleCostFunction
ObstacleCostFunction(costmap_2d::Costmap2D *costmap)
Definition: obstacle_cost_function.cpp:80
base_local_planner::ObstacleCostFunction::max_scaling_factor_
double max_scaling_factor_
Definition: obstacle_cost_function.h:155
base_local_planner
Definition: costmap_model.h:44
base_local_planner::ObstacleCostFunction::prepare
bool prepare()
Definition: obstacle_cost_function.cpp:105
base_local_planner::ObstacleCostFunction::getScalingFactor
static double getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor)
Definition: obstacle_cost_function.cpp:137
trajectory_cost_function.h
base_local_planner::ObstacleCostFunction::footprintCost
static double footprintCost(const double &x, const double &y, const double &th, double scale, std::vector< geometry_msgs::Point > footprint_spec, costmap_2d::Costmap2D *costmap, base_local_planner::WorldModel *world_model)
Definition: obstacle_cost_function.cpp:151


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