Collaboration diagram for nav2_theta_star_planner::ThetaStar:

[legend]

Public Member Functions
	ThetaStar (Parameters *params)

bool	generatePath (std::vector< coordsW > &raw_path, std::function< bool()> cancel_checker)
	it iteratively searches upon the nodes in the queue (open list) until the current node is the goal pose or the size of queue becomes 0 More...

bool	isSafe (const int &cx, const int &cy) const
	this function checks whether the cost of a point(cx, cy) on the costmap is less than or equal to the MAX_NON_OBSTACLE_COST More...

void	setStartAndGoal (const geometry_msgs::msg::PoseStamped &start, const geometry_msgs::msg::PoseStamped &goal)
	initialises the values of the start and goal points

bool	isUnsafeToPlan () const
	checks whether the start and goal points have costmap costs greater than MAX_NON_OBSTACLE_COST More...

void	clearStart ()
	Clear Start.

Public Attributes
coordsM	src_ {}

coordsM	dst_ {}

nav2_costmap_2d::Costmap2D *	costmap_ {}

int	size_x_
	the x-directional and y-directional lengths of the map respectively

int	size_y_

int	nodes_opened = 0

Protected Member Functions
void	resetParent (tree_node *curr_data)
	it performs a line of sight (los) check between the current node and the parent node of its parent node; if an los is found and the new costs calculated are lesser, then the cost and parent node of the current node is updated More...

void	setNeighbors (const tree_node *curr_data)
	this function expands the current node More...

bool	losCheck (const int &x0, const int &y0, const int &x1, const int &y1, double &sl_cost) const
	performs the line of sight check using Bresenham's Algorithm, and has been modified to calculate the traversal cost incurred in a straight line path between the two points whose coordinates are (x0, y0) and (x1, y1) More...

void	backtrace (std::vector< coordsW > &raw_points, const tree_node *curr_n) const
	it returns the path by backtracking from the goal to the start, by using their parent nodes More...

bool	isSafe (const int &cx, const int &cy, double &cost) const
	it is an overloaded function to ease the cost calculations while performing the LOS check More...

double	getCost (const int &cx, const int &cy) const

double	getTraversalCost (const int &cx, const int &cy)
	for the point(cx, cy), its traversal cost is calculated by <parameter>*(<actual_traversal_cost_from_costmap>)^2/(<max_cost>)^2 More...

double	getEuclideanCost (const int &ax, const int &ay, const int &bx, const int &by)
	calculates the piecewise straight line euclidean distances by <euc_cost_parameter>*<euclidean distance between the points (ax, ay) and (bx, by)> More...

double	getHCost (const int &cx, const int &cy)
	for the point(cx, cy), its heuristic cost is calculated by <heuristic_cost_parameter>*<euclidean distance between the point and goal> More...

bool	withinLimits (const int &cx, const int &cy) const
	checks if the given coordinates(cx, cy) lies within the map More...

bool	isGoal (const tree_node &this_node) const
	checks if the coordinates of a node is the goal or not More...

void	initializePosn (int size_inc=0)
	initialises the node_position_ vector by storing -1 as index for all points(x, y) within the limits of the map More...

void	addIndex (const int &cx, const int &cy, tree_node *node_this)
	it stores id_this in node_position_ at the index [ size_x_*cy + cx ] More...

tree_node *	getIndex (const int &cx, const int &cy)
	retrieves the pointer of the location at which the data of the point(cx, cy) is stored in nodes_data More...

void	addToNodesData (const int &id_this)
	this function depending on the size of the nodes_data_ vector allots space to store the data for a node(x, y) More...

void	resetContainers ()
	initialises the values of global variables at beginning of the execution of the generatePath function

void	clearQueue ()
	clears the priority queue after each execution of the generatePath function

Protected Attributes
std::vector< tree_node * >	node_position_

std::vector< tree_node >	nodes_data_

std::priority_queue< tree_node , std::vector< tree_node >, comp >	queue_
	this is the priority queue (open_list) to select the next node to be expanded

int	index_generated_

Parameters *	params_

const coordsM	moves [8]

tree_node *	exp_node

Detailed Description

Definition at line 62 of file theta_star.hpp.

Member Function Documentation

◆ addIndex()

void nav2_theta_star_planner::ThetaStar::addIndex	(	const int &	cx,
		const int &	cy,
		tree_node *	node_this
	)

inlineprotected

it stores id_this in node_position_ at the index [ size_x_*cy + cx ]

Parameters

id_this a pointer to the location at which the data of the point(cx, cy) is stored in nodes_data_

Definition at line 273 of file theta_star.hpp.

References node_position_, and size_x_.

◆ addToNodesData()

void nav2_theta_star_planner::ThetaStar::addToNodesData ( const int & id_this )

inlineprotected

this function depending on the size of the nodes_data_ vector allots space to store the data for a node(x, y)

Parameters

id_this is the index at which the data is stored/has to be stored for that node

Definition at line 291 of file theta_star.hpp.

References nodes_data_.

◆ backtrace()

void nav2_theta_star_planner::ThetaStar::backtrace	(	std::vector< coordsW > &	raw_points,
		const tree_node *	curr_n
	)		const

protected

it returns the path by backtracking from the goal to the start, by using their parent nodes

Parameters

raw_points	used to return the path thus found
curr_id	sends in the index of the goal coordinate, as stored in nodes_position

Definition at line 159 of file theta_star.cpp.

◆ generatePath()

bool nav2_theta_star_planner::ThetaStar::generatePath	(	std::vector< coordsW > &	raw_path,
		std::function< bool()>	cancel_checker
	)

it iteratively searches upon the nodes in the queue (open list) until the current node is the goal pose or the size of queue becomes 0

Parameters

raw_path is used to return the path obtained by executing the algorithm

Returns: true if a path is found, false if no path is found in between the start and goal pose

Definition at line 47 of file theta_star.cpp.

◆ getEuclideanCost()

double nav2_theta_star_planner::ThetaStar::getEuclideanCost	(	const int &	ax,
		const int &	ay,
		const int &	bx,
		const int &	by
	)

inlineprotected

calculates the piecewise straight line euclidean distances by <euc_cost_parameter>*<euclidean distance between the points (ax, ay) and (bx, by)>

Returns: the distance thus calculated

Definition at line 230 of file theta_star.hpp.

References nav2_theta_star_planner::Parameters::w_euc_cost.

◆ getHCost()

double nav2_theta_star_planner::ThetaStar::getHCost	(	const int &	cx,
		const int &	cy
	)

inlineprotected

for the point(cx, cy), its heuristic cost is calculated by <heuristic_cost_parameter>*<euclidean distance between the point and goal>

Returns: the heuristic cost

Definition at line 240 of file theta_star.hpp.

References nav2_theta_star_planner::Parameters::w_heuristic_cost.

◆ getIndex()

tree_node* nav2_theta_star_planner::ThetaStar::getIndex	(	const int &	cx,
		const int &	cy
	)

inlineprotected

retrieves the pointer of the location at which the data of the point(cx, cy) is stored in nodes_data

Returns: id_this is the pointer to that location

Definition at line 282 of file theta_star.hpp.

References node_position_, and size_x_.

Referenced by nav2_simple_commander.costmap_2d.PyCostmap2D::getCostXY(), nav2_simple_commander.occupancy_grid.PyOccupancyGrid::getCostXY(), nav2_simple_commander.occupancy_grid.PyOccupancyGrid::setCost(), and nav2_simple_commander.costmap_2d.PyCostmap2D::setCost().

Here is the caller graph for this function:

◆ getTraversalCost()

double nav2_theta_star_planner::ThetaStar::getTraversalCost	(	const int &	cx,
		const int &	cy
	)

inlineprotected

for the point(cx, cy), its traversal cost is calculated by <parameter>*(<actual_traversal_cost_from_costmap>)^2/(<max_cost>)^2

Returns: the traversal cost thus calculated

Definition at line 218 of file theta_star.hpp.

References nav2_theta_star_planner::Parameters::w_traversal_cost.

◆ initializePosn()

void nav2_theta_star_planner::ThetaStar::initializePosn ( int size_inc = 0 )

protected

initialises the node_position_ vector by storing -1 as index for all points(x, y) within the limits of the map

Parameters

size_inc is used to increase the number of elements in node_position_ in case the size of the map increases

Definition at line 232 of file theta_star.cpp.

◆ isGoal()

bool nav2_theta_star_planner::ThetaStar::isGoal ( const tree_node & this_node ) const

inlineprotected

checks if the coordinates of a node is the goal or not

Returns: the result of the check

Definition at line 258 of file theta_star.hpp.

◆ isSafe() [1/2]

bool nav2_theta_star_planner::ThetaStar::isSafe	(	const int &	cx,
		const int &	cy
	)		const

inline

this function checks whether the cost of a point(cx, cy) on the costmap is less than or equal to the MAX_NON_OBSTACLE_COST

Returns: the result of the check

Definition at line 86 of file theta_star.hpp.

References nav2_theta_star_planner::Parameters::allow_unknown, and nav2_costmap_2d::Costmap2D::getCost().

Referenced by isUnsafeToPlan().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isSafe() [2/2]

bool nav2_theta_star_planner::ThetaStar::isSafe	(	const int &	cx,
		const int &	cy,
		double &	cost
	)		const

inlineprotected

it is an overloaded function to ease the cost calculations while performing the LOS check

Parameters

cost	denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned

Returns: false if the traversal cost is greater than the MAX_NON_OBSTACLE_COST and true otherwise

Definition at line 187 of file theta_star.hpp.

References nav2_theta_star_planner::Parameters::allow_unknown, nav2_costmap_2d::Costmap2D::getCost(), and nav2_theta_star_planner::Parameters::w_traversal_cost.

Here is the call graph for this function:

◆ isUnsafeToPlan()

bool nav2_theta_star_planner::ThetaStar::isUnsafeToPlan ( ) const

inline

checks whether the start and goal points have costmap costs greater than MAX_NON_OBSTACLE_COST

Returns: true if the cost of any one of the points is greater than MAX_NON_OBSTACLE_COST

Definition at line 103 of file theta_star.hpp.

References isSafe().

Here is the call graph for this function:

◆ losCheck()

bool nav2_theta_star_planner::ThetaStar::losCheck	(	const int &	x0,
		const int &	y0,
		const int &	x1,
		const int &	y1,
		double &	sl_cost
	)		const

protected

performs the line of sight check using Bresenham's Algorithm, and has been modified to calculate the traversal cost incurred in a straight line path between the two points whose coordinates are (x0, y0) and (x1, y1)

Parameters

sl_cost is used to return the cost thus incurred

Returns: true if a line of sight exists between the points

Definition at line 179 of file theta_star.cpp.

◆ resetParent()

void nav2_theta_star_planner::ThetaStar::resetParent ( tree_node * curr_data )

protected

it performs a line of sight (los) check between the current node and the parent node of its parent node; if an los is found and the new costs calculated are lesser, then the cost and parent node of the current node is updated

Parameters

data	of the current node

Definition at line 91 of file theta_star.cpp.

◆ setNeighbors()

void nav2_theta_star_planner::ThetaStar::setNeighbors ( const tree_node * curr_data )

protected

this function expands the current node

Parameters

curr_data	used to send the data of the current node
curr_id	used to send the index of the current node as stored in nodes_position_

Definition at line 110 of file theta_star.cpp.

◆ withinLimits()

bool nav2_theta_star_planner::ThetaStar::withinLimits	(	const int &	cx,
		const int &	cy
	)		const

inlineprotected

checks if the given coordinates(cx, cy) lies within the map

Returns: the result of the check

Definition at line 249 of file theta_star.hpp.

Member Data Documentation

◆ index_generated_

int nav2_theta_star_planner::ThetaStar::index_generated_

protected

it is a counter like variable used to generate consecutive indices such that the data for all the nodes (in open and closed lists) could be stored consecutively in nodes_data_

Definition at line 134 of file theta_star.hpp.

◆ moves

const coordsM nav2_theta_star_planner::ThetaStar::moves[8]

protected

Initial value:

= {{0, 1},
    {0, -1},
    {1, 0},
    {-1, 0},
    {1, -1},
    {-1, 1},
    {1, 1},
    {-1, -1}}

Definition at line 138 of file theta_star.hpp.

◆ node_position_

std::vector<tree_node *> nav2_theta_star_planner::ThetaStar::node_position_

protected

for the coordinates (x,y), it stores at node_position_[size_x_ * y + x], the pointer to the location at which the data of the node is present in nodes_data_ it is initialised with size_x_ * size_y_ elements and its number of elements increases to account for a change in map size

Definition at line 120 of file theta_star.hpp.

Referenced by addIndex(), and getIndex().

◆ nodes_data_

std::vector<tree_node> nav2_theta_star_planner::ThetaStar::nodes_data_

protected

the vector nodes_data_ stores the coordinates, costs and index of the parent node, and whether or not the node is present in queue_, for all the nodes searched it is initialised with no elements and its size increases depending on the number of nodes searched

Definition at line 126 of file theta_star.hpp.

Referenced by addToNodesData().

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

nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp
nav2_theta_star_planner/src/theta_star.cpp

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

◆ addIndex()

◆ addToNodesData()

◆ backtrace()

◆ generatePath()

◆ getEuclideanCost()

◆ getHCost()

◆ getIndex()

◆ getTraversalCost()

◆ initializePosn()

◆ isGoal()

◆ isSafe() [1/2]

◆ isSafe() [2/2]

◆ isUnsafeToPlan()

◆ losCheck()

◆ resetParent()

◆ setNeighbors()

◆ withinLimits()

Member Data Documentation

◆ index_generated_

◆ moves

◆ node_position_

◆ nodes_data_