Collaboration diagram for theta_star::ThetaStar:

[legend]

Public Member Functions
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 the LETHAL_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 LETHAL_COST More...

void	clearStart ()
	Clear Start.

Public Attributes
coordsM	src_ {}

coordsM	dst_ {}

nav2_costmap_2d::Costmap2D *	costmap_ {}

double	w_traversal_cost_
	weight on the costmap traversal cost

double	w_euc_cost_
	weight on the euclidean distance cost (used for calculations of g_cost)

double	w_heuristic_cost_
	weight on the heuristic cost (used for h_cost calculations)

int	how_many_corners_
	parameter to set the number of adjacent nodes to be searched on

bool	allow_unknown_
	parameter to set weather the planner can plan through unknown space

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

int	size_y_

int	terminal_checking_interval_
	the interval at which the planner checks if it has been cancelled

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_

const coordsM	moves [8]

tree_node *	exp_node

Detailed Description

Definition at line 61 of file theta_star.hpp.

Member Function Documentation

◆ addIndex()

void theta_star::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 280 of file theta_star.hpp.

References node_position_, and size_x_.

◆ addToNodesData()

void theta_star::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 298 of file theta_star.hpp.

References nodes_data_.

◆ backtrace()

void theta_star::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 160 of file theta_star.cpp.

◆ generatePath()

bool theta_star::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 48 of file theta_star.cpp.

◆ getEuclideanCost()

double theta_star::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 237 of file theta_star.hpp.

References w_euc_cost_.

◆ getHCost()

double theta_star::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 247 of file theta_star.hpp.

References w_heuristic_cost_.

◆ getIndex()

tree_node* theta_star::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 289 of file theta_star.hpp.

References node_position_, and size_x_.

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

Here is the caller graph for this function:

◆ getTraversalCost()

double theta_star::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 226 of file theta_star.hpp.

References w_traversal_cost_.

◆ initializePosn()

void theta_star::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 256 of file theta_star.cpp.

◆ isGoal()

bool theta_star::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 265 of file theta_star.hpp.

◆ isSafe() [1/2]

bool theta_star::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 the LETHAL_COST

Returns: the result of the check

Definition at line 98 of file theta_star.hpp.

References 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 theta_star::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 / equal to the LETHAL_COST and true otherwise

Definition at line 198 of file theta_star.hpp.

References allow_unknown_, nav2_costmap_2d::Costmap2D::getCost(), and w_traversal_cost_.

Here is the call graph for this function:

◆ isUnsafeToPlan()

bool theta_star::ThetaStar::isUnsafeToPlan ( ) const

inline

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

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

Definition at line 116 of file theta_star.hpp.

References isSafe().

Here is the call graph for this function:

◆ losCheck()

bool theta_star::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 180 of file theta_star.cpp.

◆ resetParent()

void theta_star::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 92 of file theta_star.cpp.

◆ setNeighbors()

void theta_star::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 111 of file theta_star.cpp.

◆ withinLimits()

bool theta_star::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 256 of file theta_star.hpp.

Member Data Documentation

◆ index_generated_

int theta_star::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 147 of file theta_star.hpp.

◆ moves

const coordsM theta_star::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 149 of file theta_star.hpp.

◆ node_position_

std::vector<tree_node *> theta_star::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 133 of file theta_star.hpp.

Referenced by addIndex(), and getIndex().

◆ nodes_data_

std::vector<tree_node> theta_star::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 139 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_