goal_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/goal_dist.h>
37 #include <nav_2d_utils/path_ops.h>
38 #include <vector>
39 
40 namespace dwb_critics
41 {
42 bool GoalDistCritic::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();
47 
48  unsigned int local_goal_x, local_goal_y;
49  if (!getLastPoseOnCostmap(global_plan, local_goal_x, local_goal_y))
50  {
51  return false;
52  }
53 
54  // Enqueue just the last pose
55  cell_values_.setValue(local_goal_x, local_goal_y, 0.0);
56  queue_->enqueueCell(local_goal_x, local_goal_y);
57 
59 
60  return true;
61 }
62 
63 bool GoalDistCritic::getLastPoseOnCostmap(const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y)
64 {
66  const nav_grid::NavGridInfo& info = costmap.getInfo();
67  nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
68  bool started_path = false;
69 
70  // skip global path points until we reach the border of the local map
71  for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); ++i)
72  {
73  double g_x = adjusted_global_plan.poses[i].x;
74  double g_y = adjusted_global_plan.poses[i].y;
75  unsigned int map_x, map_y;
76  if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION)
77  {
78  // Still on the costmap. Continue.
79  x = map_x;
80  y = map_y;
81  started_path = true;
82  }
83  else if (started_path)
84  {
85  // Off the costmap after being on the costmap. Return the last saved indices.
86  return true;
87  }
88  // else, we have not yet found a point on the costmap, so we just continue
89  }
90 
91  if (started_path)
92  {
93  return true;
94  }
95  else
96  {
97  ROS_ERROR_NAMED("GoalDistCritic", "None of the points of the global plan were in the local costmap.");
98  return false;
99  }
100 }
101 
102 } // namespace dwb_critics
103 
void reset() override
Clear the queue and set cell_values_ to the appropriate number of unreachableCellScore.
Definition: map_grid.cpp:94
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
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: goal_dist.cpp:42
Scores trajectories based on how far along the global path they end up.
Definition: goal_dist.h:50
nav_core2::Costmap::Ptr costmap_
bool getLastPoseOnCostmap(const nav_2d_msgs::Path2D &global_plan, unsigned int &x, unsigned int &y)
Definition: goal_dist.cpp:63
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