#include <SBPLPlanner2D.h>
Public Member Functions | |
gridmap_2d::GridMap2DPtr | getMap () const |
const nav_msgs::Path & | getPath () const |
double | getPathCosts () const |
double | getRobotRadius () const |
void | goalCallback (const geometry_msgs::PoseStampedConstPtr &goal) |
Set goal and plan when start was already set. | |
void | mapCallback (const nav_msgs::OccupancyGridConstPtr &occupancy_map) |
calls updateMap() | |
bool | plan (const geometry_msgs::Pose &start, const geometry_msgs::Pose &goal) |
Plans from start to goal, assuming that the map was set with updateMap(). When successful, you can retrieve the path with getPath(). | |
bool | plan (double startX, double startY, double goalX, double goalY) |
Plans from start to goal, assuming that the map was set with updateMap(). When successful, you can retrieve the path with getPath(). | |
SBPLPlanner2D () | |
void | startCallback (const geometry_msgs::PoseWithCovarianceStampedConstPtr &start) |
Set start and plan when goal was already set. | |
bool | updateMap (gridmap_2d::GridMap2DPtr map) |
Setup the internal map representation and initialize the SBPL planning environment. | |
virtual | ~SBPLPlanner2D () |
Protected Member Functions | |
bool | plan () |
void | setPlanner () |
(re)sets the planner | |
Protected Attributes | |
double | allocated_time_ |
bool | forward_search_ |
geometry_msgs::Pose | goal_pose_ |
bool | goal_received_ |
ros::Subscriber | goal_sub_ |
double | initial_epsilon_ |
gridmap_2d::GridMap2DPtr | map_ |
ros::Subscriber | map_sub_ |
ros::NodeHandle | nh_ |
nav_msgs::Path | path_ |
double | path_costs_ |
ros::Publisher | path_pub_ |
boost::shared_ptr< SBPLPlanner > | planner_ |
boost::shared_ptr < EnvironmentNAV2D > | planner_environment_ |
std::string | planner_type_ |
double | robot_radius_ |
bool | search_until_first_solution_ |
geometry_msgs::Pose | start_pose_ |
bool | start_received_ |
ros::Subscriber | start_sub_ |
Static Protected Attributes | |
static const unsigned char | OBSTACLE_COST = 20 |
Definition at line 41 of file SBPLPlanner2D.h.
Definition at line 31 of file SBPLPlanner2D.cpp.
SBPLPlanner2D::~SBPLPlanner2D | ( | ) | [virtual] |
Definition at line 52 of file SBPLPlanner2D.cpp.
gridmap_2d::GridMap2DPtr SBPLPlanner2D::getMap | ( | ) | const [inline] |
Definition at line 54 of file SBPLPlanner2D.h.
const nav_msgs::Path& SBPLPlanner2D::getPath | ( | ) | const [inline] |
Definition at line 77 of file SBPLPlanner2D.h.
double SBPLPlanner2D::getPathCosts | ( | ) | const [inline] |
Definition at line 75 of file SBPLPlanner2D.h.
double SBPLPlanner2D::getRobotRadius | ( | ) | const [inline] |
Definition at line 78 of file SBPLPlanner2D.h.
void SBPLPlanner2D::goalCallback | ( | const geometry_msgs::PoseStampedConstPtr & | goal | ) |
Set goal and plan when start was already set.
Definition at line 56 of file SBPLPlanner2D.cpp.
void SBPLPlanner2D::mapCallback | ( | const nav_msgs::OccupancyGridConstPtr & | occupancy_map | ) |
calls updateMap()
Definition at line 192 of file SBPLPlanner2D.cpp.
bool SBPLPlanner2D::plan | ( | const geometry_msgs::Pose & | start, |
const geometry_msgs::Pose & | goal | ||
) |
Plans from start to goal, assuming that the map was set with updateMap(). When successful, you can retrieve the path with getPath().
start | |
goal |
Definition at line 86 of file SBPLPlanner2D.cpp.
bool SBPLPlanner2D::plan | ( | double | startX, |
double | startY, | ||
double | goalX, | ||
double | goalY | ||
) |
Plans from start to goal, assuming that the map was set with updateMap(). When successful, you can retrieve the path with getPath().
Definition at line 96 of file SBPLPlanner2D.cpp.
bool SBPLPlanner2D::plan | ( | ) | [protected] |
Definition at line 109 of file SBPLPlanner2D.cpp.
void SBPLPlanner2D::setPlanner | ( | ) | [protected] |
(re)sets the planner
Definition at line 223 of file SBPLPlanner2D.cpp.
void SBPLPlanner2D::startCallback | ( | const geometry_msgs::PoseWithCovarianceStampedConstPtr & | start | ) |
Set start and plan when goal was already set.
Definition at line 71 of file SBPLPlanner2D.cpp.
bool SBPLPlanner2D::updateMap | ( | gridmap_2d::GridMap2DPtr | map | ) |
Setup the internal map representation and initialize the SBPL planning environment.
Definition at line 197 of file SBPLPlanner2D.cpp.
double SBPLPlanner2D::allocated_time_ [protected] |
Definition at line 91 of file SBPLPlanner2D.h.
bool SBPLPlanner2D::forward_search_ [protected] |
Definition at line 94 of file SBPLPlanner2D.h.
geometry_msgs::Pose SBPLPlanner2D::goal_pose_ [protected] |
Definition at line 98 of file SBPLPlanner2D.h.
bool SBPLPlanner2D::goal_received_ [protected] |
Definition at line 97 of file SBPLPlanner2D.h.
ros::Subscriber SBPLPlanner2D::goal_sub_ [protected] |
Definition at line 84 of file SBPLPlanner2D.h.
double SBPLPlanner2D::initial_epsilon_ [protected] |
Definition at line 92 of file SBPLPlanner2D.h.
gridmap_2d::GridMap2DPtr SBPLPlanner2D::map_ [protected] |
Definition at line 88 of file SBPLPlanner2D.h.
ros::Subscriber SBPLPlanner2D::map_sub_ [protected] |
Definition at line 84 of file SBPLPlanner2D.h.
ros::NodeHandle SBPLPlanner2D::nh_ [protected] |
Definition at line 83 of file SBPLPlanner2D.h.
const unsigned char SBPLPlanner2D::OBSTACLE_COST = 20 [static, protected] |
Definition at line 102 of file SBPLPlanner2D.h.
nav_msgs::Path SBPLPlanner2D::path_ [protected] |
Definition at line 99 of file SBPLPlanner2D.h.
double SBPLPlanner2D::path_costs_ [protected] |
Definition at line 100 of file SBPLPlanner2D.h.
ros::Publisher SBPLPlanner2D::path_pub_ [protected] |
Definition at line 85 of file SBPLPlanner2D.h.
boost::shared_ptr<SBPLPlanner> SBPLPlanner2D::planner_ [protected] |
Definition at line 86 of file SBPLPlanner2D.h.
boost::shared_ptr<EnvironmentNAV2D> SBPLPlanner2D::planner_environment_ [protected] |
Definition at line 87 of file SBPLPlanner2D.h.
std::string SBPLPlanner2D::planner_type_ [protected] |
Definition at line 90 of file SBPLPlanner2D.h.
double SBPLPlanner2D::robot_radius_ [protected] |
Definition at line 95 of file SBPLPlanner2D.h.
bool SBPLPlanner2D::search_until_first_solution_ [protected] |
Definition at line 93 of file SBPLPlanner2D.h.
geometry_msgs::Pose SBPLPlanner2D::start_pose_ [protected] |
Definition at line 98 of file SBPLPlanner2D.h.
bool SBPLPlanner2D::start_received_ [protected] |
Definition at line 97 of file SBPLPlanner2D.h.
ros::Subscriber SBPLPlanner2D::start_sub_ [protected] |
Definition at line 84 of file SBPLPlanner2D.h.