Plugin-based global wavefront planner that conforms to the `nav_core2` GlobalPlanner interface. More...
#include <dlux_global_planner.h>
Public Member Functions | |
DluxGlobalPlanner () | |
void | initialize (const ros::NodeHandle &parent, const std::string &name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override |
virtual bool | isPlanValid (const nav_2d_msgs::Path2D &path) const |
Check the costmap for any obstacles on this path. | |
nav_2d_msgs::Path2D | makePlan (const nav_2d_msgs::Pose2DStamped &start, const nav_2d_msgs::Pose2DStamped &goal) override |
Protected Member Functions | |
virtual bool | hasValidCachedPath (const geometry_msgs::Pose2D &local_goal, unsigned int goal_x, unsigned int goal_y) |
Check whether there is a valid cached path where the goal hasn't changed. | |
virtual bool | shouldReturnCachedPathImmediately () const |
Whether the planner should always return a valid cached path without running the planning algorithm. | |
virtual bool | shouldReturnNewPath (const nav_2d_msgs::Path2D &new_path, const double new_path_cost) const |
Given a cached path is available and a new path, should the new path be the one returned? | |
Protected Attributes | |
unsigned int | cached_goal_x_ |
unsigned int | cached_goal_y_ |
nav_2d_msgs::Path2D | cached_path_ |
double | cached_path_cost_ |
pluginlib::ClassLoader < PotentialCalculator > | calc_loader_ |
boost::shared_ptr < PotentialCalculator > | calculator_ |
CostInterpreter::Ptr | cost_interpreter_ |
nav_core2::Costmap::Ptr | costmap_ |
double | improvement_threshold_ |
bool | path_caching_ |
PotentialGrid | potential_grid_ |
nav_grid_pub_sub::ScaleGridPublisher < float > | potential_pub_ |
bool | print_statistics_ |
TFListenerPtr | tf_ |
boost::shared_ptr< Traceback > | traceback_ |
pluginlib::ClassLoader< Traceback > | traceback_loader_ |
Plugin-based global wavefront planner that conforms to the `nav_core2` GlobalPlanner interface.
Definition at line 55 of file dlux_global_planner.h.
Definition at line 45 of file dlux_global_planner.cpp.
bool dlux_global_planner::DluxGlobalPlanner::hasValidCachedPath | ( | const geometry_msgs::Pose2D & | local_goal, |
unsigned int | goal_x, | ||
unsigned int | goal_y | ||
) | [protected, virtual] |
Check whether there is a valid cached path where the goal hasn't changed.
The precise goal and the grid coordinates of the goal are passed (redundantly) so the implementing method can decide what precision to require for the goal to be the same. The default implementation uses the grid coords.
Also sets the cached goal data member(s)
local_goal | Precise (floating point) goal |
goal_x | x coordinate of the goal in the grid |
goal_y | y coordinate of the goal in the grid |
Definition at line 169 of file dlux_global_planner.cpp.
void dlux_global_planner::DluxGlobalPlanner::initialize | ( | const ros::NodeHandle & | parent, |
const std::string & | name, | ||
TFListenerPtr | tf, | ||
nav_core2::Costmap::Ptr | costmap | ||
) | [override] |
Definition at line 52 of file dlux_global_planner.cpp.
bool dlux_global_planner::DluxGlobalPlanner::isPlanValid | ( | const nav_2d_msgs::Path2D & | path | ) | const [virtual] |
Check the costmap for any obstacles on this path.
path | Path to check |
Definition at line 85 of file dlux_global_planner.cpp.
nav_2d_msgs::Path2D dlux_global_planner::DluxGlobalPlanner::makePlan | ( | const nav_2d_msgs::Pose2DStamped & | start, |
const nav_2d_msgs::Pose2DStamped & | goal | ||
) | [override, virtual] |
Implements nav_core2::GlobalPlanner.
Definition at line 101 of file dlux_global_planner.cpp.
bool dlux_global_planner::DluxGlobalPlanner::shouldReturnCachedPathImmediately | ( | ) | const [protected, virtual] |
Whether the planner should always return a valid cached path without running the planning algorithm.
Definition at line 179 of file dlux_global_planner.cpp.
bool dlux_global_planner::DluxGlobalPlanner::shouldReturnNewPath | ( | const nav_2d_msgs::Path2D & | new_path, |
const double | new_path_cost | ||
) | const [protected, virtual] |
Given a cached path is available and a new path, should the new path be the one returned?
new_path | The new path |
new_path_cost | The cost of the new path, according to the traceback |
Definition at line 185 of file dlux_global_planner.cpp.
unsigned int dlux_global_planner::DluxGlobalPlanner::cached_goal_x_ [protected] |
Definition at line 122 of file dlux_global_planner.h.
unsigned int dlux_global_planner::DluxGlobalPlanner::cached_goal_y_ [protected] |
Definition at line 122 of file dlux_global_planner.h.
nav_2d_msgs::Path2D dlux_global_planner::DluxGlobalPlanner::cached_path_ [protected] |
Definition at line 121 of file dlux_global_planner.h.
double dlux_global_planner::DluxGlobalPlanner::cached_path_cost_ [protected] |
Definition at line 123 of file dlux_global_planner.h.
pluginlib::ClassLoader<PotentialCalculator> dlux_global_planner::DluxGlobalPlanner::calc_loader_ [protected] |
Definition at line 107 of file dlux_global_planner.h.
boost::shared_ptr<PotentialCalculator> dlux_global_planner::DluxGlobalPlanner::calculator_ [protected] |
Definition at line 108 of file dlux_global_planner.h.
Definition at line 116 of file dlux_global_planner.h.
nav_core2::Costmap::Ptr dlux_global_planner::DluxGlobalPlanner::costmap_ [protected] |
Definition at line 113 of file dlux_global_planner.h.
double dlux_global_planner::DluxGlobalPlanner::improvement_threshold_ [protected] |
Definition at line 120 of file dlux_global_planner.h.
bool dlux_global_planner::DluxGlobalPlanner::path_caching_ [protected] |
Definition at line 119 of file dlux_global_planner.h.
PotentialGrid dlux_global_planner::DluxGlobalPlanner::potential_grid_ [protected] |
Definition at line 115 of file dlux_global_planner.h.
nav_grid_pub_sub::ScaleGridPublisher<float> dlux_global_planner::DluxGlobalPlanner::potential_pub_ [protected] |
Definition at line 126 of file dlux_global_planner.h.
bool dlux_global_planner::DluxGlobalPlanner::print_statistics_ [protected] |
Definition at line 129 of file dlux_global_planner.h.
TFListenerPtr dlux_global_planner::DluxGlobalPlanner::tf_ [protected] |
Definition at line 114 of file dlux_global_planner.h.
boost::shared_ptr<Traceback> dlux_global_planner::DluxGlobalPlanner::traceback_ [protected] |
Definition at line 110 of file dlux_global_planner.h.
pluginlib::ClassLoader<Traceback> dlux_global_planner::DluxGlobalPlanner::traceback_loader_ [protected] |
Definition at line 109 of file dlux_global_planner.h.