Understanding the Code

The code consists of a struct GraphNode and three classes ,viz , Color,PathVisualizer and AStarPlanner. Each of them has been explained one by one in the following section of the page.

Struct GraphNode

It consists of :

• an unsigned integer which is used for indices later in the code
• an Eigen::Vector3d point which is simply a column vector having three double values.
• a vector of pointers of the struct which is used to contain the info about neighbours
• a function to add neighbours to a particular GraphNode
• a constructor which has
  • the first argument as a PointXYZI from pcl library which simply is a collection of 3 coordinate values and one intensity value
  • the second argument is an unsigned integer.

Class Color

It is a class inherited from std_msgs::ColorRGBA class which consists of 4 float values.

This class has 3 constructors:

• a default one
• one which initialises RGB values
• and the last which initialises RGBA values

This class consists of functions like white, black ,etc which return constructors having initialised the RGB values corresponding to them .

The functions of this class are used to decide that in which color do we want the vertices/edges of our graph and path to be visualized inside rviz using visualizeGraph and visualizePath.

Class PathVisualizer

It is used to visualize the path as the name suggests.

It has the following functions:

> init function

It takes the nodeHandles as it's arguments. It fetches the parameter visualize and voxel_size from the server. It creates colour maps for the colours like white,black,etc. Their first arguments are values like 0,1,2.. and the second arguments are the functions white,black,etc.

> createPublisher function

It takes the reference of a string as an argument.It makes a publisher object whose message type is visualization_msgs::MarkerArray and the topic name is the same as the argument of the function.It then creates a map the topic and the publisher.

> visualizeGraph function

This function is used to visualize the graph(whose smallest unit is struct Graphnode) generated by function createGraph which is a part of class AStarPlanner.

If the graph is empty then the function ends simply and does nothing.

For a non-empty graph it does the following things:

• It creates a variable markers of the message type visualization_msgs::MarkerArray and uses this markers as the second entry to create a publish map (publisher_map_) whose first entry is topic_name .

• The markers contains vertex_marker and edge_marker which are of message type visualization_msgs::Marker.

  • vertex_marker ,as the name suggests, is used to visualize vertices of the graph.To understand the different initialization of it's fields (which is included in the code) you can have a look at this .For every unit node of the graph, it copies the values of the three coordinates from it's position_ variable into a geometry_msgs::Point variable and pushes it into the vertex_marker. After doing the aforementioned step with every node, it simply pushes vertex_marker into markers.

  • edge_marker ,as the name suggests, is used to visualize the edges connecting the nodes of the graph.It initializes different fields of this marker in a similar way as did for vertex_marker. For every unit node in the graph,it copies the values of the three coordinates from it's position_ variable into a geometry_msgs::Point variable start_point and the coordinates of all of it's neighbours are also copied into variables called end_point and pushed into edge_marker. Finally the edge_marker is pushed into markers.

• Eventually the markers is published.

> visualizePath function

This function is used to visualize the path found by the function findPath.

If the path is empty it simply ends and does nothing.

For a non-empty path it does the following things:

• A marker is created of the message type visualization_msgs::Marker.It is initialised in a similar fashion as edge_marker in the previous function because this marker is also going to give us some curve and not vertices.
• The path is a vector of Eigen::Vector3d points. For every point of the path, it copies it's three coordinates into a variables of type geometry_msgs::Point and pushes them into points field of the marker.
• A markers is created of the message type visualization_msgs::MarkerArray and then the marker is pushed into it.
• Eventually markers is published after it has been incorporated into publisher_map_ as done into visualizeGraph.

Class AStarPlanner

This class has a constructor and a few functions which are described below:

> Constructor AStarPlanner

It initializes some variables,fetches some parameters,creates some publishers,some subscribers and some services and sets up some planners and other functions.

In particular,it does the following things:

• fetches parameters like robot_radius ,visualize and frame_id from parameter server.
• creates the following publishers:
  • path_marker_pub to publish the markers for visualizing the path.
  • sparse_graph_pub to publish the markers for visualizing the graph generated.
  • esdf_slice_pub_ to publish the pointCloud of points of type pcl::PointXYZI.
  • waypoint_list_pub to publish the list of points that constitute the path.
• creates a subscriber esdf_slice_sub_ to subscribe to the topic on which the slices are being published.
• creates the following services:
  • planner_srv_ which responds when requested to plan the path using PlannerServiceCallback.
  • path_pub_srv_ which responds when requested to publish the path using publishPathCallback.
• sets the traversability radius which simply means minimum distance from the obstacles that our robot should maintain in order to avoid collisions.
• sets up the skeleton generator
• sets up the A* planner
• sets up the shortener.

> createGraph function

It takes two points start and end as it's arguments of the type Eigen::Vector3d and then copies them into Start and End of the type pcl::PointXYZI.

It clears the graph graph_ first so that nothing is inside it beforehand and then pushes Start and End into it.Then iteratively it pushes all those points from pointCloud whose intensity values are greater than robot_radius_ into the graph.

Then it clears the tree_ (which is an Rtree) .For every node of the graph_ it takes their coordinates and make pairs with their id_ and pushes them inside the tree_.

Then for each node of the graph_ it does the following things:

• It creates a vector of a data type which is a pair of a Point from boost library and an unsigned integer and calls it neighbours.
• It copies the coordinates of the position_ of the node into the Point and pushes 5 nearest points into neighbours.
• Then for each neighbour from neighbours ,if it's id_ is not equal to the second argument of the neighbour ,then adds into the neighbour field of the node by using the addNeighbour function.

> searchPath function

This function is used to search the path based on A* search algorithm.

The basic idea of A* algorithm is explained below:

• At each step it picks the node according to a value- f which is a parameter equal to the sum of two other parameters – g and h.
• The first parameeter g is a movement cost parameter that is used to calculate the distance from starting point to the current location.
• The second parameter h is a heuristic parameter that is to calculate our estimated distance from the current location to the end location.
• The h parameter can be calculated using Euclidean distance , Manhattan distance , Diagonal distance, etc. In our case we use the Euclidean one.

The containers that has been used in the code for this are :

• f_score_map which is a pair of double value and an unsigned int value to calculate the f parameter.
• open_set which is a priority_queue of f_score_map which keeps it sorted into non-decreasing order.
• g_score which is vector of doubles and keeps record of the g parameter.
• parent which is a vector of unsigned integers which keeps the record of the previous index of the current node.

> findPath function

This function is used to find the path as the name suggests.

It first creates the graph using createGraph, visualize it using visualizeGraph and eventually searches the path between the points using searcPath.

> generateSparseGraph function

This function is used to generate a sparse graph. A sparse graph is a graph in which the number of edges is much less than the possible number of edges.

It first updates the skeleton from layer after removing any pre-existing points in the skeleton.Then it calls the generateSparseGraph function which is in voxblox::SkeletonGenerator and is different from the generateSparseGraph of class AStarPlanner.

It then visualizes the graph using visualization_msgs::MarkerArray and publishing it using the publisher sparse_graph_pub_ on the topic sparse_graph.

> skeletonize function

This function is used to make a skeleton from the esdf layer.

It does the following things:

• sets the esdf layer.
• gets the minimum separation angle ( Minimum separation angle between two edges of a sparse graph node in radians) from getMinSeparationAngle function which is used to set the default value of min_seperation_angle. Then it fetches it from the parameter server and uses it to set the min_seperation_angle using setMinSeperationAngle function.
• In the very similar fashion as the previous point, generate_by_layer_neighbors ( which decides whether to generate vertices/edges by number of basis points (false, default) or number of neighbors on the discretized medial axis (true) ) and num_neighbors_for_edge are processed.
• Finally it sets the minimum GVD distance ( GVD stands for Generalized Voronoi Diagram which is commonly used as a safe roadmap in the context of path planning and it is used to compute distance between robots and points in the environment).

> plannerServiceCallback function

This function responds by planning the path once requested to do so using rviz.

It does the following things :

• generates the skeleton
• generates the sparse graph
• finds the path
• creates a marker_array of the type visualization_msgs::MarkerArray in order to visualize the generated path inside rviz
• gets the path using Esdf and diagram
• shortens the path if possible
• publishes the marker_array using path_marker_pub
• and finally visualizes the path inside rviz.

> publishPathCallback function

This funtion is used to publish waypoints of the path searched. It creates a variable pose_array of the type geometry_msgs::PoseArray. For every point in the last_waypoints_ (a variable of type mav_msgs::EigenTrajectoryPointVector) ,it creates a variable pose_stamped of type geometry_msgs::PoseStamped ,uses the point and the pose_stamped to create a pose and finally pushes it inside the pose_array.

Finally it publishes the pose_array using waypoint_list_pub_ on the topic waypoint_list.

> esdfSliceCallback function

It takes a pointcloud and publishes it using the esdf_slice_pub_ publisher on the esdf_slice_out topic.

Then it declares and initializes start and end points of the type Eigen::Vector3d and passes them as arguments into createGraph to create the graph.

It finally calls the visualizeGraph function which visualizes the graph into rviz.