path_dist.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, 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 #include <dwb_critics/path_dist.h>
37 #include <nav_2d_utils/path_ops.h>
38 #include <vector>
39 
40 namespace dwb_critics
41 {
42 bool PathDistCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
43  const geometry_msgs::Pose2D& goal,
44  const nav_2d_msgs::Path2D& global_plan)
45 {
46  reset();
48  const nav_grid::NavGridInfo& info = costmap.getInfo();
49  bool started_path = false;
50 
51  nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
52 
53  if (adjusted_global_plan.poses.size() != global_plan.poses.size())
54  {
55  ROS_DEBUG_NAMED("PathDistCritic", "Adjusted global plan resolution, added %zu points",
56  adjusted_global_plan.poses.size() - global_plan.poses.size());
57  }
58 
59  unsigned int i;
60  // put global path points into local map until we reach the border of the local map
61  for (i = 0; i < adjusted_global_plan.poses.size(); ++i)
62  {
63  double g_x = adjusted_global_plan.poses[i].x;
64  double g_y = adjusted_global_plan.poses[i].y;
65  unsigned int map_x, map_y;
66  if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION)
67  {
68  cell_values_.setValue(map_x, map_y, 0.0);
69  queue_->enqueueCell(map_x, map_y);
70  started_path = true;
71  }
72  else if (started_path)
73  {
74  break;
75  }
76  }
77  if (!started_path)
78  {
79  ROS_ERROR_NAMED("PathDistCritic",
80  "None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
81  i, adjusted_global_plan.poses.size(), global_plan.poses.size());
82  return false;
83  }
84 
86 
87  return true;
88 }
89 
90 } // namespace dwb_critics
91 
void reset() override
Clear the queue and set cell_values_ to the appropriate number of unreachableCellScore.
Definition: map_grid.cpp:94
bool prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel, const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) override
Definition: path_dist.cpp:42
nav_core2::BasicCostmap costmap
void propogateManhattanDistances()
Go through the queue and set the cells to the Manhattan distance from their parents.
Definition: map_grid.cpp:110
static const unsigned char NO_INFORMATION
#define ROS_DEBUG_NAMED(name,...)
Scores trajectories based on how far from the global path they end up.
Definition: path_dist.h:45
nav_core2::Costmap::Ptr costmap_
void setValue(const unsigned int x, const unsigned int y, const T &value) override
nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D &global_plan_in, double resolution)
#define ROS_ERROR_NAMED(name,...)
std::shared_ptr< MapGridQueue > queue_
Definition: map_grid.h:126
nav_grid::VectorNavGrid< double > cell_values_
Definition: map_grid.h:127
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)


dwb_critics
Author(s): David V. Lu!!
autogenerated on Sun Jan 10 2021 04:08:44