regular codes

README.md

@@ -271,8 +271,8 @@ Explanation:
 |    **LPA\***     |    [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/lpa_star.cpp))     |     ![lpa_star_ros.gif](assets/lpa_star_ros.gif)      |
 |   **D\* Lite**   |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/d_star_lite.cpp))   |  ![d_star_lite_ros.gif](assets/d_star_lite_ros.gif)   |
 |   **Voronoi**    |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/voronoi.cpp))     |      ![voronoi_ros.gif](assets/voronoi_ros.gif)       |
-|   **Theta\***    |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/theta_star.cpp))    |   ![Status](https://img.shields.io/badge/gif-none-yellow)    |
-| **Lazy Theta\*** |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/lazy_theta_star.cpp))    | ![Status](https://img.shields.io/badge/gif-none-yellow) |
+|   **Theta\***    |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/theta_star.cpp))    |   ![theta_star_ros.gif](assets/theta_star_ros.gif)    |
+| **Lazy Theta\*** |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)]((https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/graph_planner/src/lazy_theta_star.cpp))    | ![lazy_theta_star_ros.gif](assets/lazy_theta_star_ros.gif) |
 |     **RRT**      |       [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/rrt.cpp)        |          ![rrt_ros.gif](assets/rrt_ros.gif)           |
 |    **RRT\***     |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/rrt_star.cpp)     |     ![rrt_star_ros.gif](assets/rrt_star_ros.gif)      |
 | **Informed RRT** |   [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/global_planner/sample_planner/src/informed_rrt.cpp)   | ![informed_rrt_ros.gif](assets/informed_rrt_ros.gif)  |

src/planner/global_planner/global_planner/include/global_planner.h

@@ -135,6 +135,22 @@ class GlobalPlanner
    */
   void outlineMap(unsigned char* costarr);
 
+  /**
+   * @brief Calculate distance between the 2 nodes.
+   * @param n1        Node 1
+   * @param n2        Node 2
+   * @return distance between nodes
+   */
+  double dist(const Node& node1, const Node& node2);
+
+  /**
+   * @brief Calculate the angle of x-axis between the 2 nodes.
+   * @param n1        Node 1
+   * @param n2        Node 2
+   * @return he angle of x-axis between the 2 node
+   */
+  double angle(const Node& node1, const Node& node2);
+
 protected:
   /**
    * @brief Convert closed list to path

src/planner/global_planner/global_planner/src/global_planner.cpp

@@ -158,6 +158,28 @@ void GlobalPlanner::outlineMap(unsigned char* costarr)
     *pc = costmap_2d::LETHAL_OBSTACLE;
 }
 
+/**
+ * @brief Calculate distance between the 2 nodes.
+ * @param n1        Node 1
+ * @param n2        Node 2
+ * @return distance between nodes
+ */
+double GlobalPlanner::dist(const Node& node1, const Node& node2)
+{
+  return std::hypot(node1.x_ - node2.x_, node1.y_ - node2.y_);
+}
+
+/**
+ * @brief Calculate the angle of x-axis between the 2 nodes.
+ * @param n1        Node 1
+ * @param n2        Node 2
+ * @return he angle of x-axis between the 2 node
+ */
+double GlobalPlanner::angle(const Node& node1, const Node& node2)
+{
+  return atan2(node2.y_ - node1.y_, node2.x_ - node1.x_);
+}
+
 /**
  * @brief Convert closed list to path
  * @param closed_list closed list
@@ -169,6 +191,7 @@ std::vector<Node> GlobalPlanner::_convertClosedListToPath(
     std::unordered_set<Node, NodeIdAsHash, compare_coordinates>& closed_list, const Node& start, const Node& goal)
 {
   auto current = *closed_list.find(goal);
+
   std::vector<Node> path;
   while (current != start)
   {

src/planner/global_planner/graph_planner/include/a_star.h

@@ -46,14 +46,6 @@ class AStar : public GlobalPlanner
   bool plan(const unsigned char* global_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
             std::vector<Node>& expand);
 
-  /**
-   * @brief Get the Heuristics
-   * @param node  current node
-   * @param goal  goal node
-   * @return  heuristics
-   */
-  double getHeuristics(const Node& node, const Node& goal);
-
 private:
   bool is_dijkstra_;  // using diksktra
   bool is_gbfs_;      // using greedy best first search(GBFS)

src/planner/global_planner/graph_planner/include/lazy_theta_star.h

@@ -2,9 +2,9 @@
  *
  * @file: lazy_theta_star.h
  * @breif: Contains the lazy Theta* planner class
- * @author: Wu Maojia
- * @update: 2023-8-14
- * @version: 1.0
+ * @author: Wu Maojia, Yang Haodong
+ * @update: 2023-8-26
+ * @version: 1.1
  *
  * Copyright (c) 2023， Wu Maojia
  * All rights reserved.
@@ -16,14 +16,14 @@
 
 #include <vector>
 #include <queue>
-#include "global_planner.h"
+#include "theta_star.h"
 
 namespace global_planner
 {
 /**
  * @brief Class for objects that plan using the lazy Theta* algorithm
  */
-class LazyThetaStar : public GlobalPlanner
+class LazyThetaStar : public ThetaStar
 {
 public:
   /**
@@ -57,30 +57,13 @@ class LazyThetaStar : public GlobalPlanner
   /**
    * @brief check if the parent of vertex need to be updated. if so, update it
    * @param node
-  */
-  void _setVertex(Node& node);
-
-  /**
-   * @brief Bresenham algorithm to check if there is any obstacle between parent and child
-   * @param parent
-   * @param child
-   * @return true if no obstacle, else false
    */
-  bool _lineOfSight(const Node& parent, const Node& child);
-
-  /**
-   * @brief Get the Euclidean distance between two nodes
-   * @param node  current node
-   * @param goal  goal node
-   * @return  Euclidean distance
-  */
-  double _getDistance(const Node& node, const Node& goal);
+  void _setVertex(Node& node);
 
 private:
-  const unsigned char* costs_;              // costmap copy
-  std::vector<Node> open_list_;             // open list
-  std::unordered_set<Node, NodeIdAsHash, compare_coordinates> closed_list_; // closed list
-  std::vector<Node> motion_;          // possible motions
+  const unsigned char* costs_;                                               // costmap copy
+  std::unordered_set<Node, NodeIdAsHash, compare_coordinates> closed_list_;  // closed list
+  std::vector<Node> motion_;                                                 // possible motions
 };
 }  // namespace global_planner
 #endif

src/planner/global_planner/graph_planner/include/theta_star.h

@@ -2,9 +2,9 @@
  *
  * @file: theta_star.h
  * @breif: Contains the Theta* planner class
- * @author: Wu Maojia
- * @update: 2023-8-14
- * @version: 1.0
+ * @author: Wu Maojia, Yang Haodong
+ * @update: 2023-8-26
+ * @version: 1.1
  *
  * Copyright (c) 2023， Wu Maojia
  * All rights reserved.
@@ -58,22 +58,13 @@ class ThetaStar : public GlobalPlanner
    * @brief Bresenham algorithm to check if there is any obstacle between parent and child
    * @param parent
    * @param child
+   * @param costs global costmap
    * @return true if no obstacle, else false
    */
-  bool _lineOfSight(const Node& parent, const Node& child);
-
-  /**
-   * @brief Get the Euclidean distance between two nodes
-   * @param node  current node
-   * @param goal  goal node
-   * @return  Euclidean distance
-  */
-  double _getDistance(const Node& node, const Node& goal);
+  bool _lineOfSight(const Node& parent, const Node& child, const unsigned char* costs);
 
 private:
-  const unsigned char* costs_;              // costmap copy
-  std::vector<Node> open_list_;             // open list
-  std::unordered_set<Node, NodeIdAsHash, compare_coordinates> closed_list_; // closed list
+  const unsigned char* costs_;  // costmap copy
 };
 }  // namespace global_planner
 #endif

src/planner/global_planner/graph_planner/src/a_star.cpp

@@ -69,11 +69,8 @@ bool AStar::plan(const unsigned char* global_costmap, const Node& start, const N
   const std::vector<Node> motion = getMotion();
 
   // main process
-  while (1)
+  while (!open_list.empty())
   {
-    if (open_list.empty())
-      break;
-
     // pop current node from open list
     Node current = open_list.top();
     open_list.pop();
@@ -111,7 +108,7 @@ bool AStar::plan(const unsigned char* global_costmap, const Node& start, const N
 
       // if using dijkstra implementation, do not consider heuristics cost
       if (!is_dijkstra_)
-        node_new.h_ = getHeuristics(node_new, goal);
+        node_new.h_ = dist(node_new, goal);
 
       // if using GBFS implementation, only consider heuristics cost
       if (is_gbfs_)
@@ -124,15 +121,4 @@ bool AStar::plan(const unsigned char* global_costmap, const Node& start, const N
 
   return false;
 }
-
-/**
- * @brief Get the Heuristics
- * @param node  current node
- * @param goal  goal node
- * @return  heuristics
- */
-double AStar::getHeuristics(const Node& node, const Node& goal)
-{
-  return std::hypot(node.x_ - goal.x_, node.y_ - goal.y_);
-}
 }  // namespace global_planner