obstacle_cost_function.cpp
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 
39 #include <cmath>
40 #include <Eigen/Core>
41 #include <ros/console.h>
42 
43 namespace base_local_planner {
44 
46  : costmap_(costmap), sum_scores_(false) {
47  if (costmap != NULL) {
49  }
50 }
51 
53  if (world_model_ != NULL) {
54  delete world_model_;
55  }
56 }
57 
58 
59 void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
60  // TODO: move this to prepare if possible
61  max_trans_vel_ = max_trans_vel;
62  max_scaling_factor_ = max_scaling_factor;
63  scaling_speed_ = scaling_speed;
64 }
65 
66 void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
67  footprint_spec_ = footprint_spec;
68 }
69 
71  return true;
72 }
73 
75  double cost = 0;
77  double px, py, pth;
78  if (footprint_spec_.size() == 0) {
79  // Bug, should never happen
80  ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
81  return -9;
82  }
83 
84  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
85  traj.getPoint(i, px, py, pth);
86  double f_cost = footprintCost(px, py, pth,
87  scale, footprint_spec_,
89 
90  if(f_cost < 0){
91  return f_cost;
92  }
93 
94  if(sum_scores_)
95  cost += f_cost;
96  else
97  cost = f_cost;
98  }
99  return cost;
100 }
101 
102 double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
103  double vmag = hypot(traj.xv_, traj.yv_);
104 
105  //if we're over a certain speed threshold, we'll scale the robot's
106  //footprint to make it either slow down or stay further from walls
107  double scale = 1.0;
108  if (vmag > scaling_speed) {
109  //scale up to the max scaling factor linearly... this could be changed later
110  double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
111  scale = max_scaling_factor * ratio + 1.0;
112  }
113  return scale;
114 }
115 
117  const double& x,
118  const double& y,
119  const double& th,
120  double scale,
121  std::vector<geometry_msgs::Point> footprint_spec,
122  costmap_2d::Costmap2D* costmap,
123  base_local_planner::WorldModel* world_model) {
124 
125  //check if the footprint is legal
126  // TODO: Cache inscribed radius
127  double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);
128 
129  if (footprint_cost < 0) {
130  return -6.0;
131  }
132  unsigned int cell_x, cell_y;
133 
134  //we won't allow trajectories that go off the map... shouldn't happen that often anyways
135  if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
136  return -7.0;
137  }
138 
139  double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
140 
141  return occ_cost;
142 }
143 
144 } /* namespace base_local_planner */
void setFootprint(std::vector< geometry_msgs::Point > footprint_spec)
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)=0
Subclass will implement this method to check a footprint at a given position and orientation for lega...
base_local_planner::WorldModel * world_model_
static double getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
ObstacleCostFunction(costmap_2d::Costmap2D *costmap)
void getPoint(unsigned int index, double &x, double &y, double &th) const
Get a point within the trajectory.
Definition: trajectory.cpp:47
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)
unsigned char getCost(unsigned int mx, unsigned int my) const
unsigned int getPointsSize() const
Return the number of points in the trajectory.
Definition: trajectory.cpp:77
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
Definition: world_model.h:52
void setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed)
A class that implements the WorldModel interface to provide grid based collision checks for the traje...
Definition: costmap_model.h:50
std::vector< geometry_msgs::Point > footprint_spec_
#define ROS_ERROR(...)
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:44


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49