Class ThetaStar
Defined in File theta_star.hpp
Class Documentation

class ThetaStar
Public Functions

ThetaStar()

~ThetaStar() = default

bool generatePath(std::vector<coordsW> &raw_path)
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
Public Members

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 xdirectional and ydirectional lengths of the map respectively

int size_y_

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_

ThetaStar()