Class ThetaStar

Class Documentation

class ThetaStar

Public Functions

ThetaStar()
~ThetaStar() = default
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

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

inline 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

Returns:

the result of the check

void setStartAndGoal(const geometry_msgs::msg::PoseStamped &start, const geometry_msgs::msg::PoseStamped &goal)

initialises the values of the start and goal points

inline bool isUnsafeToPlan() const

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

void clearStart()

Clear Start.

Public Members

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

Parameters:

data – of the current node

void setNeighbors(const tree_node *curr_data)

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_

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)

Parameters:

sl_cost – is used to return the cost thus incurred

Returns:

true if a line of sight exists between the points

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

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

inline 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

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

inline double getCost(const int &cx, const int &cy) const
inline 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

Returns:

the traversal cost thus calculated

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

Returns:

the distance thus calculated

inline 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>

Returns:

the heuristic cost

inline bool withinLimits(const int &cx, const int &cy) const

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

Returns:

the result of the check

inline bool isGoal(const tree_node &this_node) const

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

Returns:

the result of the check

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

Parameters:

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

inline 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 ]

Parameters:

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

inline 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

Returns:

id_this is the pointer to that location

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

Parameters:

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

void resetContainers()

initialises the values of global variables at beginning of the execution of the generatePath function

inline void clearQueue()

clears the priority queue after each execution of the generatePath function

Protected Attributes

std::vector<tree_node*> node_position_

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

std::vector<tree_node> nodes_data_

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

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_

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_

const coordsM moves[8] = {{0, 1}, {0, -1}, {1, 0}, {-1, 0}, {1, -1}, {-1, 1}, {1, 1}, {-1, -1}}
tree_node *exp_node