#include <hector_exploration_planner.h>

Public Member Functions
float	angleDifferenceWall (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal)
bool	doAlternativeExploration (const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > &plan, std::vector< geometry_msgs::PoseStamped > &oldplan)
bool	doExploration (const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > &plan)
bool	doInnerExploration (const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > &plan)
void	dynRecParamCallback (hector_exploration_planner::ExplorationPlannerConfig &config, uint32_t level)
bool	exploreWalls (const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > &goals)
bool	findFrontiers (std::vector< geometry_msgs::PoseStamped > &frontiers, std::vector< geometry_msgs::PoseStamped > &noFrontiers)
bool	findFrontiers (std::vector< geometry_msgs::PoseStamped > &frontiers)
bool	findInnerFrontier (std::vector< geometry_msgs::PoseStamped > &innerFrontier)
bool	getObservationPose (const geometry_msgs::PoseStamped &observation_pose, const double desired_distance, geometry_msgs::PoseStamped &new_observation_pose)
	HectorExplorationPlanner ()
	HectorExplorationPlanner (std::string name, costmap_2d::Costmap2DROS *costmap_ros)
void	initialize (std::string name, costmap_2d::Costmap2DROS *costmap_ros)
bool	makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &original_goal, std::vector< geometry_msgs::PoseStamped > &plan)
	~HectorExplorationPlanner ()
Private Types
enum	LastMode { FRONTIER_EXPLORE, INNER_EXPLORE }
Private Member Functions
unsigned int	angleDanger (float angle)
float	angleDifference (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal)
bool	buildexploration_trans_array_ (const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > goals, bool useAnglePenalty)
bool	buildobstacle_trans_array_ (bool use_inflated_obstacles)
unsigned int	cellDanger (int point)
void	clearFrontiers ()
void	deleteMapData ()
int	down (int point)
int	downleft (int point)
int	downright (int point)
void	getAdjacentPoints (int point, int points[])
void	getDiagonalPoints (int point, int points[])
float	getDistanceWeight (const geometry_msgs::PoseStamped &point1, const geometry_msgs::PoseStamped &point2)
void	getStraightPoints (int point, int points[])
bool	getTrajectory (const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > goals, std::vector< geometry_msgs::PoseStamped > &plan)
double	getYawToUnknown (int point)
bool	isFree (int point)
bool	isFrontier (int point)
bool	isFrontierReached (int point)
bool	isSameFrontier (int frontier_point1, int frontier_point2)
bool	isValid (int point)
int	left (int point)
bool	recoveryMakePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)
void	resetMaps ()
int	right (int point)
void	saveMaps (std::string path)
void	setupMapData ()
int	up (int point)
int	upleft (int point)
int	upright (int point)
Private Attributes
costmap_2d::Costmap2D	costmap_
costmap_2d::Costmap2DROS *	costmap_ros_
boost::shared_ptr < dynamic_reconfigure::Server < hector_exploration_planner::ExplorationPlannerConfig > >	dyn_rec_server_
unsigned int *	exploration_trans_array_
int *	frontier_map_array_
bool	initialized_
boost::shared_ptr < ExplorationTransformVis >	inner_vis_
bool *	is_goal_array_
enum hector_exploration_planner::HectorExplorationPlanner::LastMode	last_mode_
unsigned int	map_height_
unsigned int	map_width_
std::string	name
unsigned int	num_map_cells_
unsigned int *	obstacle_trans_array_
const unsigned char *	occupancy_grid_array_
double	p_alpha_
double	p_dist_for_goal_reached_
int	p_goal_angle_penalty_
int	p_min_frontier_size_
int	p_min_obstacle_dist_
bool	p_plan_in_unknown_
double	p_same_frontier_dist_
bool	p_use_inflated_obs_
ros::ServiceClient	path_service_client_
int	previous_goal_
boost::shared_ptr < ExplorationTransformVis >	vis_
ros::Publisher	visualization_pub_

Detailed Description

Definition at line 47 of file hector_exploration_planner.h.

Member Enumeration Documentation

enum hector_exploration_planner::HectorExplorationPlanner::LastMode [private]

Enumerator:

FRONTIER_EXPLORE
INNER_EXPLORE

Definition at line 92 of file hector_exploration_planner.h.

Constructor & Destructor Documentation

HectorExplorationPlanner::HectorExplorationPlanner ( )

Definition at line 46 of file hector_exploration_planner.cpp.

HectorExplorationPlanner::~HectorExplorationPlanner ( )

Definition at line 59 of file hector_exploration_planner.cpp.

HectorExplorationPlanner::HectorExplorationPlanner	(	std::string	name,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

Definition at line 63 of file hector_exploration_planner.cpp.

Member Function Documentation

unsigned int HectorExplorationPlanner::angleDanger ( float angle ) [private]

M_PI;

Definition at line 1549 of file hector_exploration_planner.cpp.

float HectorExplorationPlanner::angleDifference	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	goal
	)		`[private]`

Definition at line 1492 of file hector_exploration_planner.cpp.

float HectorExplorationPlanner::angleDifferenceWall	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	goal
	)

Definition at line 525 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::buildexploration_trans_array_	(	const geometry_msgs::PoseStamped &	start,
		std::vector< geometry_msgs::PoseStamped >	goals,
		bool	useAnglePenalty
	)		`[private]`

Definition at line 782 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::buildobstacle_trans_array_ ( bool use_inflated_obstacles ) [private]

Definition at line 887 of file hector_exploration_planner.cpp.

unsigned int HectorExplorationPlanner::cellDanger ( int point ) [inline, private]

Definition at line 1484 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::clearFrontiers ( ) [private]

Definition at line 1408 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::deleteMapData ( ) [private]

Definition at line 761 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::doAlternativeExploration	(	const geometry_msgs::PoseStamped &	start,
		std::vector< geometry_msgs::PoseStamped > &	plan,
		std::vector< geometry_msgs::PoseStamped > &	oldplan
	)

Definition at line 485 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::doExploration	(	const geometry_msgs::PoseStamped &	start,
		std::vector< geometry_msgs::PoseStamped > &	plan
	)

Given a start point, finds a frontier between known and unknown space and generates a plan to go there

Parameters:

start	The start point
plan	The plan to explore into unknown space

Definition at line 195 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::doInnerExploration	(	const geometry_msgs::PoseStamped &	start,
		std::vector< geometry_msgs::PoseStamped > &	plan
	)

This can be used if there are no frontiers to unknown space left in the map. The robot will retrieve it's path travelled so far via a service and try to go to places having a large distance to this path.

Parameters:

start	The start point
plan	The plan to explore into unknown space

Definition at line 247 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::down ( int point ) [inline, private]

Definition at line 1655 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::downleft ( int point ) [inline, private]

Definition at line 1662 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::downright ( int point ) [inline, private]

Definition at line 1648 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::dynRecParamCallback	(	hector_exploration_planner::ExplorationPlannerConfig &	config,
		uint32_t	level
	)

Definition at line 107 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::exploreWalls	(	const geometry_msgs::PoseStamped &	start,
		std::vector< geometry_msgs::PoseStamped > &	goals
	)

Definition at line 549 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::findFrontiers	(	std::vector< geometry_msgs::PoseStamped > &	frontiers,
		std::vector< geometry_msgs::PoseStamped > &	noFrontiers
	)

Definition at line 1031 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::findFrontiers ( std::vector< geometry_msgs::PoseStamped > & frontiers )

Definition at line 1022 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::findInnerFrontier ( std::vector< geometry_msgs::PoseStamped > & innerFrontier )

Definition at line 1221 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::getAdjacentPoints	(	int	point,
		int	points[]
	)		`[inline, private]`

Definition at line 1602 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::getDiagonalPoints	(	int	point,
		int	points[]
	)		`[inline, private]`

Definition at line 1577 of file hector_exploration_planner.cpp.

float HectorExplorationPlanner::getDistanceWeight	(	const geometry_msgs::PoseStamped &	point1,
		const geometry_msgs::PoseStamped &	point2
	)		`[private]`

Definition at line 1555 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::getObservationPose	(	const geometry_msgs::PoseStamped &	observation_pose,
		const double	desired_distance,
		geometry_msgs::PoseStamped &	new_observation_pose
	)

Definition at line 336 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::getStraightPoints	(	int	point,
		int	points[]
	)		`[inline, private]`

Definition at line 1568 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::getTrajectory	(	const geometry_msgs::PoseStamped &	start,
		std::vector< geometry_msgs::PoseStamped >	goals,
		std::vector< geometry_msgs::PoseStamped > &	plan
	)		`[private]`

Definition at line 947 of file hector_exploration_planner.cpp.

double HectorExplorationPlanner::getYawToUnknown ( int point ) [private]

Definition at line 1521 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::initialize	(	std::string	name,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

Definition at line 68 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::isFree ( int point ) [private]

Definition at line 1416 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::isFrontier ( int point ) [private]

Definition at line 1371 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::isFrontierReached ( int point ) [private]

Definition at line 1439 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::isSameFrontier	(	int	frontier_point1,
		int	frontier_point2
	)		`[private]`

Definition at line 1465 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::isValid ( int point ) [inline, private]

Definition at line 1412 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::left ( int point ) [inline, private]

Definition at line 1615 of file hector_exploration_planner.cpp.

bool HectorExplorationPlanner::makePlan	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	original_goal,
		std::vector< geometry_msgs::PoseStamped > &	plan
	)

Plans from start to given goal. If orientation quaternion of goal is all zeros, calls exploration instead. This is a hacky workaround that has to be refactored.

Parameters:

start	The start point
goal	The goal point (Use orientation quaternion all 0 to let exploration find goal point)
plan	The generated plan

Definition at line 143 of file hector_exploration_planner.cpp.

bool hector_exploration_planner::HectorExplorationPlanner::recoveryMakePlan	(	const geometry_msgs::PoseStamped &	start,
		const geometry_msgs::PoseStamped &	goal,
		std::vector< geometry_msgs::PoseStamped > &	plan
	)		`[private]`

void HectorExplorationPlanner::resetMaps ( ) [private]

Definition at line 1402 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::right ( int point ) [inline, private]

Definition at line 1641 of file hector_exploration_planner.cpp.

void hector_exploration_planner::HectorExplorationPlanner::saveMaps ( std::string path ) [private]

void HectorExplorationPlanner::setupMapData ( ) [private]

Updates costmap data and resizes internal data structures if costmap size has changed. Should be called once before every planning command

Definition at line 737 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::up ( int point ) [inline, private]

Definition at line 1629 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::upleft ( int point ) [inline, private]

Definition at line 1622 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::upright ( int point ) [inline, private]

Definition at line 1635 of file hector_exploration_planner.cpp.

Member Data Documentation

costmap_2d::Costmap2D hector_exploration_planner::HectorExplorationPlanner::costmap_ [private]

Definition at line 136 of file hector_exploration_planner.h.

costmap_2d::Costmap2DROS* hector_exploration_planner::HectorExplorationPlanner::costmap_ros_ [private]

Definition at line 135 of file hector_exploration_planner.h.

boost::shared_ptr<dynamic_reconfigure::Server<hector_exploration_planner::ExplorationPlannerConfig> > hector_exploration_planner::HectorExplorationPlanner::dyn_rec_server_ [private]

Definition at line 162 of file hector_exploration_planner.h.

unsigned int* hector_exploration_planner::HectorExplorationPlanner::exploration_trans_array_ [private]

Definition at line 139 of file hector_exploration_planner.h.

int* hector_exploration_planner::HectorExplorationPlanner::frontier_map_array_ [private]

Definition at line 141 of file hector_exploration_planner.h.

bool hector_exploration_planner::HectorExplorationPlanner::initialized_ [private]

Definition at line 144 of file hector_exploration_planner.h.

boost::shared_ptr<ExplorationTransformVis> hector_exploration_planner::HectorExplorationPlanner::inner_vis_ [private]

Definition at line 165 of file hector_exploration_planner.h.

bool* hector_exploration_planner::HectorExplorationPlanner::is_goal_array_ [private]

Definition at line 142 of file hector_exploration_planner.h.

enum hector_exploration_planner::HectorExplorationPlanner::LastMode hector_exploration_planner::HectorExplorationPlanner::last_mode_ [private]