#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	findFrontiersCloseToPath (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 use_cell_danger=true)
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	isFreeFrontiers (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
boost::shared_ptr < ExplorationTransformVis >	close_path_vis_
costmap_2d::Costmap2D *	costmap_
costmap_2d::Costmap2DROS *	costmap_ros_
boost::shared_ptr < dynamic_reconfigure::Server < hector_exploration_planner::ExplorationPlannerConfig > >	dyn_rec_server_
boost::shared_array< unsigned int >	exploration_trans_array_
boost::shared_array< int >	frontier_map_array_
ros::Publisher	goal_pose_pub_
bool	initialized_
boost::shared_ptr < ExplorationTransformVis >	inner_vis_
boost::shared_array< 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_
ros::Publisher	observation_pose_pub_
boost::shared_array< unsigned int >	obstacle_trans_array_
boost::shared_ptr < ExplorationTransformVis >	obstacle_vis_
const unsigned char *	occupancy_grid_array_
double	p_alpha_
double	p_close_to_path_target_distance_
double	p_cos_of_allowed_observation_pose_angle_
double	p_dist_for_goal_reached_
bool	p_explore_close_to_path_
int	p_goal_angle_penalty_
int	p_min_frontier_size_
int	p_min_obstacle_dist_
double	p_observation_pose_desired_dist_
double	p_obstacle_cutoff_dist_
bool	p_plan_in_unknown_
double	p_same_frontier_dist_
bool	p_use_inflated_obs_
bool	p_use_observation_pose_calculation_
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 93 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 55 of file hector_exploration_planner.cpp.

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

Definition at line 59 of file hector_exploration_planner.cpp.

Member Function Documentation

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

M_PI;

Definition at line 1797 of file hector_exploration_planner.cpp.

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

Definition at line 1738 of file hector_exploration_planner.cpp.

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

Definition at line 570 of file hector_exploration_planner.cpp.

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

Definition at line 836 of file hector_exploration_planner.cpp.

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

Definition at line 967 of file hector_exploration_planner.cpp.

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

Definition at line 1724 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::clearFrontiers ( ) [private]

Definition at line 1627 of file hector_exploration_planner.cpp.

void HectorExplorationPlanner::deleteMapData ( ) [private]

Definition at line 827 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 530 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 203 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 269 of file hector_exploration_planner.cpp.

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

Definition at line 1903 of file hector_exploration_planner.cpp.

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

Definition at line 1910 of file hector_exploration_planner.cpp.

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

Definition at line 1896 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 594 of file hector_exploration_planner.cpp.

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

Definition at line 1252 of file hector_exploration_planner.cpp.

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

Definition at line 1098 of file hector_exploration_planner.cpp.

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

Definition at line 1107 of file hector_exploration_planner.cpp.

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

Definition at line 1438 of file hector_exploration_planner.cpp.

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

Definition at line 1850 of file hector_exploration_planner.cpp.

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

Definition at line 1825 of file hector_exploration_planner.cpp.

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

Definition at line 1803 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 359 of file hector_exploration_planner.cpp.

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

Definition at line 1816 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 1018 of file hector_exploration_planner.cpp.

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

Definition at line 1767 of file hector_exploration_planner.cpp.

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

Definition at line 64 of file hector_exploration_planner.cpp.

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

Definition at line 1635 of file hector_exploration_planner.cpp.

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

Definition at line 1660 of file hector_exploration_planner.cpp.

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

Definition at line 1588 of file hector_exploration_planner.cpp.

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

Definition at line 1679 of file hector_exploration_planner.cpp.

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

Definition at line 1705 of file hector_exploration_planner.cpp.

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

Definition at line 1631 of file hector_exploration_planner.cpp.

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

Definition at line 1863 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 127 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 1621 of file hector_exploration_planner.cpp.

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

Definition at line 1889 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 782 of file hector_exploration_planner.cpp.

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

Definition at line 1877 of file hector_exploration_planner.cpp.

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

Definition at line 1870 of file hector_exploration_planner.cpp.

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

Definition at line 1883 of file hector_exploration_planner.cpp.

Member Data Documentation

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

Definition at line 177 of file hector_exploration_planner.h.

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

Definition at line 141 of file hector_exploration_planner.h.

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

Definition at line 140 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 174 of file hector_exploration_planner.h.

boost::shared_array<unsigned int> hector_exploration_planner::HectorExplorationPlanner::exploration_trans_array_ [private]

Definition at line 144 of file hector_exploration_planner.h.

boost::shared_array<int> hector_exploration_planner::HectorExplorationPlanner::frontier_map_array_ [private]

Definition at line 146 of file hector_exploration_planner.h.

ros::Publisher hector_exploration_planner::HectorExplorationPlanner::goal_pose_pub_ [private]

Definition at line 136 of file hector_exploration_planner.h.

bool hector_exploration_planner::HectorExplorationPlanner::initialized_ [private]

Definition at line 149 of file hector_exploration_planner.h.

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

Definition at line 178 of file hector_exploration_planner.h.

boost::shared_array<bool> hector_exploration_planner::HectorExplorationPlanner::is_goal_array_ [private]