#include <sbpl_dynamic_env_global_planner.h>
Public Member Functions | |
virtual void | initialize (std::string name, costmap_2d::Costmap2DROS *costmap_ros) |
Initialization function for the SBPLDynEnvGlobalPlanner object. | |
virtual bool | makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan) |
Given a goal pose in the world, compute a plan. | |
SBPLDynEnvGlobalPlanner (std::string name, costmap_2d::Costmap2DROS *costmap_ros) | |
Constructor for the SBPLDynEnvGlobalPlanner object. | |
SBPLDynEnvGlobalPlanner () | |
Default constructor. | |
virtual | ~SBPLDynEnvGlobalPlanner () |
Private Member Functions | |
void | dynamicObstacleCallback (const dynamic_obs_msgs::DynamicObstaclesConstPtr &msg) |
ros::Time | getDynamicObstacles () |
void | moveBaseStatusCallback (const actionlib_msgs::GoalStatusArrayConstPtr &msg) |
void | visualizeExpansions () |
Private Attributes | |
double | allocated_time_ |
costmap_2d::Costmap2D | cost_map_ |
std::string | cost_map_topic_ |
costmap_2d::Costmap2DROS * | costmap_ros_ |
vector< SBPL_DynamicObstacle_t > * | current_dynObs |
ros::Time | current_dynObs_timestamp |
double | decrease_epsilon_ |
double | dyn_obs_pad_costmap_removal |
ros::Subscriber | dynObs_sub |
DiscreteSpaceTimeIntervalInformation * | env |
std::string | environment_type_ |
std::vector< geometry_msgs::Point > | footprint_ |
int | force_scratch_limit_ |
bool | forward_search_ |
ros::Publisher | goal_pub |
double | initial_epsilon_ |
bool | initialized_ |
boost::recursive_mutex | lock_ |
pthread_mutex_t | m |
ros::Publisher | marker_pub |
ros::Subscriber | moveBaseStatus_sub |
bool | pathDone |
vector< SBPL_DynamicObstacle_t > * | plan_dynObs |
ros::Time | plan_dynObs_timestamp |
ros::Publisher | plan_pub_ |
IntervalPlanner * | planner |
std::string | planner_type_ |
geometry_msgs::PoseStamped | prevGoal |
std::string | primitive_filename_ |
bool | remove_dynObs_from_costmap |
vector< SBPL_DynamicObstacle_t > * | sensor_dynObs |
double | temporal_padding |
tf::TransformListener | tf_ |
double | time_resolution |
ROS TF
Definition at line 26 of file sbpl_dynamic_env_global_planner.h.
SBPLDynEnvGlobalPlanner::SBPLDynEnvGlobalPlanner | ( | ) |
Default constructor.
Definition at line 54 of file sbpl_dynamic_env_global_planner.cpp.
SBPLDynEnvGlobalPlanner::SBPLDynEnvGlobalPlanner | ( | std::string | name, | |
costmap_2d::Costmap2DROS * | costmap_ros | |||
) |
Constructor for the SBPLDynEnvGlobalPlanner object.
name | The name of this planner | |
costmap_ros | A pointer to the ROS wrapper of the costmap to use |
Definition at line 149 of file sbpl_dynamic_env_global_planner.cpp.
virtual SBPLDynEnvGlobalPlanner::~SBPLDynEnvGlobalPlanner | ( | ) | [inline, virtual] |
Definition at line 61 of file sbpl_dynamic_env_global_planner.h.
void SBPLDynEnvGlobalPlanner::dynamicObstacleCallback | ( | const dynamic_obs_msgs::DynamicObstaclesConstPtr & | msg | ) | [private] |
Definition at line 64 of file sbpl_dynamic_env_global_planner.cpp.
ros::Time SBPLDynEnvGlobalPlanner::getDynamicObstacles | ( | ) | [private] |
Definition at line 115 of file sbpl_dynamic_env_global_planner.cpp.
void SBPLDynEnvGlobalPlanner::initialize | ( | std::string | name, | |
costmap_2d::Costmap2DROS * | costmap_ros | |||
) | [virtual] |
Initialization function for the SBPLDynEnvGlobalPlanner object.
name | The name of this planner | |
costmap_ros | A pointer to the ROS wrapper of the costmap to use |
Definition at line 156 of file sbpl_dynamic_env_global_planner.cpp.
bool SBPLDynEnvGlobalPlanner::makePlan | ( | const geometry_msgs::PoseStamped & | start, | |
const geometry_msgs::PoseStamped & | goal, | |||
std::vector< geometry_msgs::PoseStamped > & | plan | |||
) | [virtual] |
Given a goal pose in the world, compute a plan.
start | The start pose | |
goal | The goal pose | |
plan | The plan... filled by the planner |
Definition at line 231 of file sbpl_dynamic_env_global_planner.cpp.
void SBPLDynEnvGlobalPlanner::moveBaseStatusCallback | ( | const actionlib_msgs::GoalStatusArrayConstPtr & | msg | ) | [private] |
Definition at line 136 of file sbpl_dynamic_env_global_planner.cpp.
void SBPLDynEnvGlobalPlanner::visualizeExpansions | ( | ) | [private] |
Definition at line 416 of file sbpl_dynamic_env_global_planner.cpp.
double SBPLDynEnvGlobalPlanner::allocated_time_ [private] |
amount of time allowed for search
Definition at line 87 of file sbpl_dynamic_env_global_planner.h.
costmap_2d::Costmap2D SBPLDynEnvGlobalPlanner::cost_map_ [private] |
local copy of the costmap underlying cost_map_ros_
Definition at line 104 of file sbpl_dynamic_env_global_planner.h.
std::string SBPLDynEnvGlobalPlanner::cost_map_topic_ [private] |
what type of environment in which to plan. choices are 2D and XYThetaLattice.
Definition at line 96 of file sbpl_dynamic_env_global_planner.h.
costmap_2d::Costmap2DROS* SBPLDynEnvGlobalPlanner::costmap_ros_ [private] |
the number of cells that have to be changed in the costmap to force the planner to plan from scratch even if its an incremental planner manages the cost map for us
Definition at line 103 of file sbpl_dynamic_env_global_planner.h.
vector<SBPL_DynamicObstacle_t>* SBPLDynEnvGlobalPlanner::current_dynObs [private] |
Definition at line 70 of file sbpl_dynamic_env_global_planner.h.
ros::Time SBPLDynEnvGlobalPlanner::current_dynObs_timestamp [private] |
Definition at line 72 of file sbpl_dynamic_env_global_planner.h.
double SBPLDynEnvGlobalPlanner::decrease_epsilon_ [private] |
initial epsilon for beginning the anytime search
Definition at line 89 of file sbpl_dynamic_env_global_planner.h.
double SBPLDynEnvGlobalPlanner::dyn_obs_pad_costmap_removal [private] |
Definition at line 91 of file sbpl_dynamic_env_global_planner.h.
ros::Subscriber SBPLDynEnvGlobalPlanner::dynObs_sub [private] |
Definition at line 74 of file sbpl_dynamic_env_global_planner.h.
DiscreteSpaceTimeIntervalInformation* SBPLDynEnvGlobalPlanner::env [private] |
Definition at line 81 of file sbpl_dynamic_env_global_planner.h.
std::string SBPLDynEnvGlobalPlanner::environment_type_ [private] |
Definition at line 95 of file sbpl_dynamic_env_global_planner.h.
std::vector<geometry_msgs::Point> SBPLDynEnvGlobalPlanner::footprint_ [private] |
Definition at line 112 of file sbpl_dynamic_env_global_planner.h.
int SBPLDynEnvGlobalPlanner::force_scratch_limit_ [private] |
where to find the motion primitives for the current robot
Definition at line 100 of file sbpl_dynamic_env_global_planner.h.
bool SBPLDynEnvGlobalPlanner::forward_search_ [private] |
what topic is being used for the costmap topic
Definition at line 98 of file sbpl_dynamic_env_global_planner.h.
ros::Publisher SBPLDynEnvGlobalPlanner::goal_pub [private] |
Definition at line 108 of file sbpl_dynamic_env_global_planner.h.
double SBPLDynEnvGlobalPlanner::initial_epsilon_ [private] |
initial epsilon for beginning the anytime search
Definition at line 88 of file sbpl_dynamic_env_global_planner.h.
bool SBPLDynEnvGlobalPlanner::initialized_ [private] |
Definition at line 78 of file sbpl_dynamic_env_global_planner.h.
boost::recursive_mutex SBPLDynEnvGlobalPlanner::lock_ [private] |
Lock for access to class members in callbacks
Definition at line 114 of file sbpl_dynamic_env_global_planner.h.
pthread_mutex_t SBPLDynEnvGlobalPlanner::m [private] |
Definition at line 116 of file sbpl_dynamic_env_global_planner.h.
ros::Publisher SBPLDynEnvGlobalPlanner::marker_pub [private] |
Definition at line 107 of file sbpl_dynamic_env_global_planner.h.
ros::Subscriber SBPLDynEnvGlobalPlanner::moveBaseStatus_sub [private] |
Definition at line 75 of file sbpl_dynamic_env_global_planner.h.
bool SBPLDynEnvGlobalPlanner::pathDone [private] |
Definition at line 110 of file sbpl_dynamic_env_global_planner.h.
vector<SBPL_DynamicObstacle_t>* SBPLDynEnvGlobalPlanner::plan_dynObs [private] |
Definition at line 71 of file sbpl_dynamic_env_global_planner.h.
ros::Time SBPLDynEnvGlobalPlanner::plan_dynObs_timestamp [private] |
Definition at line 73 of file sbpl_dynamic_env_global_planner.h.
ros::Publisher SBPLDynEnvGlobalPlanner::plan_pub_ [private] |
Definition at line 106 of file sbpl_dynamic_env_global_planner.h.
IntervalPlanner* SBPLDynEnvGlobalPlanner::planner [private] |
Definition at line 80 of file sbpl_dynamic_env_global_planner.h.
std::string SBPLDynEnvGlobalPlanner::planner_type_ [private] |
sbpl method to use for planning. choices are ARAPlanner and ADPlanner
Definition at line 85 of file sbpl_dynamic_env_global_planner.h.
geometry_msgs::PoseStamped SBPLDynEnvGlobalPlanner::prevGoal [private] |
Definition at line 109 of file sbpl_dynamic_env_global_planner.h.
std::string SBPLDynEnvGlobalPlanner::primitive_filename_ [private] |
whether to use forward or backward search
Definition at line 99 of file sbpl_dynamic_env_global_planner.h.
bool SBPLDynEnvGlobalPlanner::remove_dynObs_from_costmap [private] |
Definition at line 93 of file sbpl_dynamic_env_global_planner.h.
vector<SBPL_DynamicObstacle_t>* SBPLDynEnvGlobalPlanner::sensor_dynObs [private] |
Definition at line 69 of file sbpl_dynamic_env_global_planner.h.
double SBPLDynEnvGlobalPlanner::temporal_padding [private] |
Definition at line 92 of file sbpl_dynamic_env_global_planner.h.
tf::TransformListener SBPLDynEnvGlobalPlanner::tf_ [private] |
for our costmap_2d::Costmap2DROS instance
Definition at line 83 of file sbpl_dynamic_env_global_planner.h.
double SBPLDynEnvGlobalPlanner::time_resolution [private] |
Definition at line 90 of file sbpl_dynamic_env_global_planner.h.