#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 )

inlineprivate

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 )

inlineprivate

Definition at line 1903 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::downleft ( int point )

inlineprivate

Definition at line 1910 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::downright ( int point )

inlineprivate

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[]
	)

inlineprivate

Definition at line 1850 of file hector_exploration_planner.cpp.

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

inlineprivate

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[]
	)

inlineprivate

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 )

inlineprivate

Definition at line 1631 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::left ( int point )

inlineprivate

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 )

inlineprivate

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 )

inlineprivate

Definition at line 1877 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::upleft ( int point )

inlineprivate

Definition at line 1870 of file hector_exploration_planner.cpp.

int HectorExplorationPlanner::upright ( int point )

inlineprivate

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

Definition at line 147 of file hector_exploration_planner.h.

enum hector_exploration_planner::HectorExplorationPlanner::LastMode hector_exploration_planner::HectorExplorationPlanner::last_mode_

private

unsigned int hector_exploration_planner::HectorExplorationPlanner::map_height_

private

Definition at line 154 of file hector_exploration_planner.h.

unsigned int hector_exploration_planner::HectorExplorationPlanner::map_width_

private

Definition at line 153 of file hector_exploration_planner.h.

std::string hector_exploration_planner::HectorExplorationPlanner::name

private

Definition at line 152 of file hector_exploration_planner.h.

unsigned int hector_exploration_planner::HectorExplorationPlanner::num_map_cells_

private

Definition at line 155 of file hector_exploration_planner.h.

ros::Publisher hector_exploration_planner::HectorExplorationPlanner::observation_pose_pub_

private

Definition at line 135 of file hector_exploration_planner.h.

boost::shared_array<unsigned int> hector_exploration_planner::HectorExplorationPlanner::obstacle_trans_array_

private

Definition at line 145 of file hector_exploration_planner.h.

boost::shared_ptr<ExplorationTransformVis> hector_exploration_planner::HectorExplorationPlanner::obstacle_vis_

private

Definition at line 179 of file hector_exploration_planner.h.

const unsigned char* hector_exploration_planner::HectorExplorationPlanner::occupancy_grid_array_

private

Definition at line 143 of file hector_exploration_planner.h.

double hector_exploration_planner::HectorExplorationPlanner::p_alpha_

private

Definition at line 164 of file hector_exploration_planner.h.

double hector_exploration_planner::HectorExplorationPlanner::p_close_to_path_target_distance_

private

Definition at line 172 of file hector_exploration_planner.h.

double hector_exploration_planner::HectorExplorationPlanner::p_cos_of_allowed_observation_pose_angle_

private

Definition at line 171 of file hector_exploration_planner.h.

double hector_exploration_planner::HectorExplorationPlanner::p_dist_for_goal_reached_

private

Definition at line 165 of file hector_exploration_planner.h.

bool hector_exploration_planner::HectorExplorationPlanner::p_explore_close_to_path_

private

Definition at line 159 of file hector_exploration_planner.h.

int hector_exploration_planner::HectorExplorationPlanner::p_goal_angle_penalty_

private

Definition at line 161 of file hector_exploration_planner.h.

int hector_exploration_planner::HectorExplorationPlanner::p_min_frontier_size_

private

Definition at line 163 of file hector_exploration_planner.h.

int hector_exploration_planner::HectorExplorationPlanner::p_min_obstacle_dist_

private

Definition at line 162 of file hector_exploration_planner.h.

double hector_exploration_planner::HectorExplorationPlanner::p_observation_pose_desired_dist_

private

Definition at line 169 of file hector_exploration_planner.h.

double hector_exploration_planner::HectorExplorationPlanner::p_obstacle_cutoff_dist_

private

Definition at line 167 of file hector_exploration_planner.h.

bool hector_exploration_planner::HectorExplorationPlanner::p_plan_in_unknown_

private

Definition at line 158 of file hector_exploration_planner.h.

double hector_exploration_planner::HectorExplorationPlanner::p_same_frontier_dist_

private

Definition at line 166 of file hector_exploration_planner.h.

bool hector_exploration_planner::HectorExplorationPlanner::p_use_inflated_obs_

private

Definition at line 160 of file hector_exploration_planner.h.

bool hector_exploration_planner::HectorExplorationPlanner::p_use_observation_pose_calculation_

private

Definition at line 168 of file hector_exploration_planner.h.

ros::ServiceClient hector_exploration_planner::HectorExplorationPlanner::path_service_client_

private

Definition at line 139 of file hector_exploration_planner.h.

int hector_exploration_planner::HectorExplorationPlanner::previous_goal_

private

Definition at line 150 of file hector_exploration_planner.h.

boost::shared_ptr<ExplorationTransformVis> hector_exploration_planner::HectorExplorationPlanner::vis_

private

Definition at line 176 of file hector_exploration_planner.h.

ros::Publisher hector_exploration_planner::HectorExplorationPlanner::visualization_pub_

private

Definition at line 138 of file hector_exploration_planner.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation