cost_interpreter.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
36 #include <algorithm>
37 #include <limits>
38 #include <stdexcept>
39 #include <string>
40 
41 namespace dlux_global_planner
42 {
44 {
45  costmap_ = costmap;
46  int neutral_cost;
47  nh.param("neutral_cost", neutral_cost, 50);
48  if (neutral_cost < 0 || neutral_cost > std::numeric_limits<unsigned char>::max())
49  {
50  throw std::invalid_argument("neutral_cost (" + std::to_string(neutral_cost) + ") must be a valid unsigned char!");
51  }
52 
53  float scale;
54  // The default value matches navfn's default value
55  // https://github.com/ros-planning/navigation/blob/8ea5462f5395acf9741253ff38302d29175dd870/global_planner/cfg/GlobalPlanner.cfg#L10
56  nh.param("scale", scale, 3.0f);
57 
59  if (nh.hasParam("unknown_interpretation"))
60  {
61  if (nh.hasParam("allow_unknown"))
62  {
63  ROS_ERROR("allow_unknown can't be specified at the same time as unknown_interpretation.");
64  ROS_ERROR("Using the value of unknown_interpretation.");
65  }
66  std::string unknown_str;
67  nh.getParam("unknown_interpretation", unknown_str);
68  if (unknown_str == "lethal")
69  {
71  }
72  else if (unknown_str == "expensive")
73  {
75  }
76  else if (unknown_str == "free")
77  {
79  }
80  else
81  {
82  ROS_ERROR("Unknown value for unknown_interpretation '%s'. Using expensive instead.", unknown_str.c_str());
84  }
85  }
86 
87  setConfiguration(static_cast<unsigned char>(neutral_cost), scale, mode);
88 }
89 
90 void CostInterpreter::setConfiguration(const unsigned char neutral_cost, const float scale,
91  const UnknownInterpretation mode)
92 {
93  neutral_cost_ = neutral_cost;
94  for (unsigned int i = 0; i < cached_costs_.size(); i++)
95  {
97  {
98  float c;
99  switch (mode)
100  {
102  c = LETHAL_COST_F;
103  break;
105  c = LETHAL_COST_F - 1.0;
106  break;
107  default: // case FREE:
108  c = neutral_cost_;
109  break;
110  }
111  cached_costs_[i] = c;
112  }
113  else if (i <= LETHAL_COST - 1)
114  {
115  float c = i * scale + neutral_cost_;
116  cached_costs_[i] = std::min(c, LETHAL_COST_F - 1.0f);
117  }
118  else
119  {
121  }
122  }
123 }
124 
125 
126 } // namespace dlux_global_planner
void setConfiguration(const unsigned char neutral_cost, const float scale, const UnknownInterpretation mode)
f
const float LETHAL_COST_F
static const unsigned char NO_INFORMATION
const unsigned char LETHAL_COST
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void initialize(ros::NodeHandle &nh, nav_core2::Costmap::Ptr costmap)
std::array< float, 256 > cached_costs_
bool getParam(const std::string &key, std::string &s) const
std::shared_ptr< Costmap > Ptr
bool hasParam(const std::string &key) const
#define ROS_ERROR(...)


dlux_global_planner
Author(s):
autogenerated on Sun Jan 10 2021 04:08:52