29 #ifndef HUMANOID_PLANNER_2D_SBPL_2D_PLANNER_ 30 #define HUMANOID_PLANNER_2D_SBPL_2D_PLANNER_ 33 #include <geometry_msgs/PoseStamped.h> 34 #include <geometry_msgs/PoseWithCovarianceStamped.h> 35 #include <sbpl/headers.h> 36 #include <visualization_msgs/Marker.h> 37 #include <nav_msgs/Path.h> 46 void goalCallback(
const geometry_msgs::PoseStampedConstPtr& goal);
50 void mapCallback(
const nav_msgs::OccupancyGridConstPtr& occupancy_map);
64 bool plan(
const geometry_msgs::Pose& start,
const geometry_msgs::Pose& goal);
72 bool plan(
double startX,
double startY,
double goalX,
double goalY);
void goalCallback(const geometry_msgs::PoseStampedConstPtr &goal)
Set goal and plan when start was already set.
geometry_msgs::Pose goal_pose_
const nav_msgs::Path & getPath() const
ros::Subscriber goal_sub_
ros::Subscriber start_sub_
bool search_until_first_solution_
gridmap_2d::GridMap2DPtr getMap() const
void setPlanner()
(re)sets the planner
static const unsigned char OBSTACLE_COST
void mapCallback(const nav_msgs::OccupancyGridConstPtr &occupancy_map)
calls updateMap()
std::string planner_type_
bool updateMap(gridmap_2d::GridMap2DPtr map)
Setup the internal map representation and initialize the SBPL planning environment.
boost::shared_ptr< EnvironmentNAV2D > planner_environment_
boost::shared_ptr< SBPLPlanner > planner_
void startCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &start)
Set start and plan when goal was already set.
geometry_msgs::Pose start_pose_
gridmap_2d::GridMap2DPtr map_
double getRobotRadius() const
double getPathCosts() const