fix bugs: core dump in getLookAheadPoint function

src/core/global_planner/sample_planner/include/rrt_star.h

@@ -51,7 +51,7 @@ class RRTStar : public RRT
    * @param node     sample node
    * @return nearest node
    */
-  Node _findNearestPoint(std::unordered_map<int, Node> list, Node& node);
+  Node _findNearestPoint(std::unordered_map<int, Node>& list, Node& node);
 
 protected:
   double r_;  // optimization radius

src/core/global_planner/sample_planner/include/sample_planner.h

@@ -127,17 +127,18 @@ class SamplePlanner : public nav_core::BaseGlobalPlanner
   ros::ServiceServer make_plan_srv_;                          // planning service
 
 private:
-  double tolerance_;     // tolerance
-  bool is_outline_;      // whether outline the boudary of map
-  double factor_;        // obstacle inflation factor
-  bool is_expand_;       // whether publish expand map or not
-  int sample_points_;    // random sample points
-  double sample_max_d_;  // max distance between sample points
-  double opt_r_;         // optimization raidus
-  double prior_set_r_;   // radius of priority sample circles
-  int rewire_threads_n_; // threads number of rewire process
-  double step_ext_d_;    // increased distance of adaptive extend step size
-  double t_freedom_;     // freedom of t distribution
+  double tolerance_;                                      // tolerance
+  bool is_outline_;                                       // whether outline the boudary of map
+  double factor_;                                         // obstacle inflation factor
+  bool is_expand_;                                        // whether publish expand map or not
+  int sample_points_;                                     // random sample points
+  double sample_max_d_;                                   // max distance between sample points
+  double opt_r_;                                          // optimization raidus
+  double prior_set_r_;                                    // radius of priority sample circles
+  int rewire_threads_n_;                                  // threads number of rewire process
+  double step_ext_d_;                                     // increased distance of adaptive extend step size
+  double t_freedom_;                                      // freedom of t distribution
+  std::vector<geometry_msgs::PoseStamped> history_plan_;  // history plan
 };
 }  // namespace sample_planner
 #endif

src/core/global_planner/sample_planner/src/quick_informed_rrt.cpp

@@ -26,14 +26,15 @@ namespace global_planner
  * @param   costmap   the environment for path planning
  * @param   max_dist    max distance between sample points
  */
-QuickInformedRRT::QuickInformedRRT(costmap_2d::Costmap2D* costmap, int sample_num, double max_dist, double r, double r_set, int n_threads, double d_extend, double t_freedom)
+QuickInformedRRT::QuickInformedRRT(costmap_2d::Costmap2D* costmap, int sample_num, double max_dist, double r,
+                                   double r_set, int n_threads, double d_extend, double t_freedom)
   : InformedRRT(costmap, sample_num, max_dist, r)
 {
   set_r_ = r_set;
-  rewire_threads_   = n_threads;
-  step_extend_d_    = d_extend;
+  rewire_threads_ = n_threads;
+  step_extend_d_ = d_extend;
   recover_max_dist_ = max_dist;
-  t_distr_freedom_  = t_freedom;
+  t_distr_freedom_ = t_freedom;
 }
 
 /**
@@ -68,7 +69,7 @@ bool QuickInformedRRT::plan(const Node& start, const Node& goal, std::vector<Nod
     iteration++;
 
     // update probability of sampling bias
-    opti_sample_p_ = std::min(0.75, 1 - dist_m2g/dist_s2g);
+    opti_sample_p_ = std::min(0.75, 1 - dist_m2g / dist_s2g);
 
     // generate a random node in the map
     Node sample_node = _generateRandomNode(mu, path);
@@ -93,7 +94,8 @@ bool QuickInformedRRT::plan(const Node& start, const Node& goal, std::vector<Nod
 
     auto dist_ = helper::dist(new_node, goal_);
     // update min dist from tree to goal
-    if (dist_ < dist_m2g) dist_m2g = dist_;
+    if (dist_ < dist_m2g)
+      dist_m2g = dist_;
     // goal found
     if (dist_ <= max_dist_ && !_isAnyObstacleInPath(new_node, goal_))
     {
@@ -110,10 +112,10 @@ bool QuickInformedRRT::plan(const Node& start, const Node& goal, std::vector<Nod
         mu = std::fmin(5, mu + 0.5);
       }
     }
-    else // if do not update, decrease mu termly
+    else  // if do not update, decrease mu termly
     {
-      mu_cnt ++;
-      if(mu_cnt % 100 == 0)
+      mu_cnt++;
+      if (mu_cnt % 100 == 0)
         mu = std::fmax(0, mu - 0.5);
     }
   }
@@ -140,15 +142,15 @@ Node QuickInformedRRT::_generateRandomNode(int mu, std::vector<Node> path)
   std::mt19937 eng(rd());
   std::student_t_distribution<> t_distr(t_distr_freedom_);
 
-  if(std::abs(t_distr(eng)) < mu && path.size() != 0) // sample in priority circles
+  if (std::abs(t_distr(eng)) < mu && path.size() != 0)  // sample in priority circles
   {
-      int wc = rand() % path.size();
-      std::uniform_real_distribution<float> p(-set_r_, set_r_);
-      int cx = path[wc].x_ + p(eng);
-      int cy = path[wc].y_ + p(eng);
-      return Node(cx, cy, 0, 0, grid2Index(cx, cy), 0);
+    int wc = rand() % path.size();
+    std::uniform_real_distribution<float> p(-set_r_, set_r_);
+    int cx = path[wc].x_ + p(eng);
+    int cy = path[wc].y_ + p(eng);
+    return Node(cx, cy, 0, 0, grid2Index(cx, cy), 0);
   }
-  else // ellipse sample
+  else  // ellipse sample
   {
     if (c_best_ < std::numeric_limits<double>::max())
     {
@@ -159,15 +161,15 @@ Node QuickInformedRRT::_generateRandomNode(int mu, std::vector<Node> path)
         std::uniform_real_distribution<float> p(-1, 1);
         while (true)
         {
-            x = p(eng);
-            y = p(eng);
-            if (x * x + y * y < 1)
+          x = p(eng);
+          y = p(eng);
+          if (x * x + y * y < 1)
             break;
         }
         // transform to ellipse
         Node temp = _transform(x, y);
         if (temp.id_ < map_size_ - 1)
-            return temp;
+          return temp;
       }
     }
     else
@@ -220,26 +222,28 @@ Node QuickInformedRRT::_findNearestPoint(std::unordered_map<int, Node> list, Nod
 
     // parallel rewire optimization
     std::vector<int> v_iters;
-    for (auto p : sample_list_) {
+    for (auto p : sample_list_)
+    {
       v_iters.push_back(p.first);
     }
 
-#   pragma omp parallel for num_threads(rewire_threads_)
+#pragma omp parallel for num_threads(rewire_threads_)
     for (int i = 0; i < v_iters.size(); i++)
     {
-      auto &p = sample_list_[v_iters[i]];
+      auto& p = sample_list_[v_iters[i]];
       // inside the optimization circle
       double new_dist = helper::dist(p, new_node);
-      if (new_dist > r_) continue;
-
+      if (new_dist > r_)
+        continue;
+
       double cost = p.g_ + new_dist;
       // update new sample node's cost and parent
       if (new_node.g_ > cost)
       {
         if (!_isAnyObstacleInPath(new_node, p))
         {
-          // other thread may update new_node.g_ 
-#         pragma omp critical
+          // other thread may update new_node.g_
+#pragma omp critical
           if (new_node.g_ > cost)
           {
             new_node.pid_ = p.id_;

src/core/global_planner/sample_planner/src/rrt_star.cpp

@@ -79,7 +79,6 @@ bool RRTStar::plan(const Node& start, const Node& goal, std::vector<Node>& path,
       path = _convertClosedListToPath(sample_list_, start, goal);
       optimized = true;
     }
-
     iteration++;
   }
 
@@ -92,12 +91,11 @@ bool RRTStar::plan(const Node& start, const Node& goal, std::vector<Node>& path,
  * @param node     sample node
  * @return nearest node
  */
-Node RRTStar::_findNearestPoint(std::unordered_map<int, Node> list, Node& node)
+Node RRTStar::_findNearestPoint(std::unordered_map<int, Node>& list, Node& node)
 {
   Node nearest_node, new_node(node);
   double min_dist = std::numeric_limits<double>::max();
-
-  for (const auto p : list)
+  for (const auto& p : list)
   {
     // calculate distance
     double new_dist = helper::dist(p.second, new_node);
@@ -128,7 +126,7 @@ Node RRTStar::_findNearestPoint(std::unordered_map<int, Node> list, Node& node)
   if (!_isAnyObstacleInPath(new_node, nearest_node))
   {
     // rewire optimization
-    for (auto p : sample_list_)
+    for (auto& p : sample_list_)
     {
       // inside the optimization circle
       double new_dist = helper::dist(p.second, new_node);

src/core/global_planner/sample_planner/src/sample_planner.cpp

@@ -198,12 +198,18 @@ bool SamplePlanner::makePlan(const geometry_msgs::PoseStamped& start, const geom
       geometry_msgs::PoseStamped goalCopy = goal;
       goalCopy.header.stamp = ros::Time::now();
       plan.push_back(goalCopy);
+      history_plan_ = plan;
     }
     else
       ROS_ERROR("Failed to get a plan from path when a legal path was found. This shouldn't happen.");
   }
+  else if (history_plan_.size() > 0) {
+    plan = history_plan_;
+    ROS_WARN("Using history path.");
+  }
   else
     ROS_ERROR("Failed to get a path.");
+
   // publish expand zone
   if (is_expand_)
     _publishExpand(expand);

src/core/local_planner/local_planner/src/local_planner.cpp

@@ -251,17 +251,14 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS
     return helper::dist(ps, robot_pose_global) >= lookahead_dist;
   });
 
-  std::vector<geometry_msgs::PoseStamped>::const_iterator prev_pose_it;
-  std::vector<geometry_msgs::PoseStamped>::const_iterator pprev_pose_it;
   // If the no pose is not far enough, take the last pose
   if (goal_pose_it == prune_plan.end())
   {
     goal_pose_it = std::prev(prune_plan.end());
-    prev_pose_it = std::prev(goal_pose_it);
-    pprev_pose_it = std::prev(prev_pose_it);
     pt.point.x = goal_pose_it->pose.position.x;
     pt.point.y = goal_pose_it->pose.position.y;
     kappa = 0;
+    theta = atan2(pt.point.y - ry, pt.point.x - rx);
   }
   else
   {
@@ -270,13 +267,20 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS
     // This can be found with a closed form for the intersection of a segment and a circle
     // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
     // and goal_pose is guaranteed to be outside the circle.
-    prev_pose_it = std::prev(goal_pose_it);
-    pprev_pose_it = std::prev(prev_pose_it);
-
-    double px = prev_pose_it->pose.position.x;
-    double py = prev_pose_it->pose.position.y;
+    double px, py;
     double gx = goal_pose_it->pose.position.x;
     double gy = goal_pose_it->pose.position.y;
+    if (goal_pose_it == prune_plan.begin())
+    {
+      px = rx;
+      py = ry;
+    }
+    else
+    {
+      auto prev_pose_it = std::prev(goal_pose_it);
+      px = prev_pose_it->pose.position.x;
+      py = prev_pose_it->pose.position.y;
+    }
 
     // transform to the robot frame so that the circle centers at (0,0)
     std::pair<double, double> prev_p(px - rx, py - ry);
@@ -286,30 +290,36 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS
     pt.point.x = i_points[0].first + rx;
     pt.point.y = i_points[0].second + ry;
 
-    double ax = pprev_pose_it->pose.position.x;
-    double ay = pprev_pose_it->pose.position.y;
-    double bx = prev_pose_it->pose.position.x;
-    double by = prev_pose_it->pose.position.y;
-    double cx = goal_pose_it->pose.position.x;
-    double cy = goal_pose_it->pose.position.y;
-
-    double a = std::hypot(bx - cx, by - cy);
-    double b = std::hypot(cx - ax, cy - ay);
-    double c = std::hypot(ax - bx, ay - by);
-
-    double cosB = (a * a + c * c - b * b) / (2 * a * c);
-    double sinB = std::sin(std::acos(cosB));
-
-    double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax);
-    kappa = std::copysign(2 * sinB / b, cross);
+    auto next_pose_it = std::next(goal_pose_it);
+    if (next_pose_it != prune_plan.end())
+    {
+      double ax = px;
+      double ay = py;
+      double bx = gx;
+      double by = gy;
+      double cx = next_pose_it->pose.position.x;
+      double cy = next_pose_it->pose.position.y;
+      double a = std::hypot(bx - cx, by - cy);
+      double b = std::hypot(cx - ax, cy - ay);
+      double c = std::hypot(ax - bx, ay - by);
+
+      double cosB = (a * a + c * c - b * b) / (2 * a * c);
+      double sinB = std::sin(std::acos(cosB));
+
+      double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax);
+      kappa = std::copysign(2 * sinB / b, cross);
+    }
+    else
+    {
+      kappa = 0.0;
+    }
+    theta = atan2(gy - py, gx - px);
   }
+
   if (std::isnan(kappa))
     kappa = 0.0;
   pt.header.frame_id = goal_pose_it->header.frame_id;
   pt.header.stamp = goal_pose_it->header.stamp;
-
-  theta = atan2(goal_pose_it->pose.position.y - prev_pose_it->pose.position.y,
-                goal_pose_it->pose.position.x - prev_pose_it->pose.position.x);
 }
 
 }  // namespace local_planner

src/core/local_planner/lqr_planner/src/lqr_planner.cpp

@@ -227,10 +227,6 @@ bool LQRPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
     Eigen::Vector3d s_d(lookahead_pt.point.x, lookahead_pt.point.y, theta_trj);                // desired state
     Eigen::Vector2d u_r(vt, vt * kappa);                                                       // refered input
     Eigen::Vector2d u = _lqrControl(s, s_d, u_r);
-    // ROS_WARN("[INPUT] vt: %f, kappa: %f, s_x: %f, s_y: %f, s_t: %f, s_d_x: %f, s_d_y: %f, s_d_t: %f", vt, kappa,
-    // s[0],
-    //          s[1], s[2], s_d[0], s_d[1], s_d[2]);
-    // ROS_WARN("ux: %f, uy: %f", u[0], u[1]);
 
     cmd_vel.linear.x = linearRegularization(base_odom, u[0]);
     cmd_vel.angular.z = angularRegularization(base_odom, u[1]);

src/core/utils/include/nodes.h

@@ -7,9 +7,9 @@
  * @date: 2023-07-21
  * @version: 2.1
  *
- * Copyright (c) 2024, Yang Haodong. 
+ * Copyright (c) 2024, Yang Haodong.
  * All rights reserved.
- * 
+ *
  * --------------------------------------------------------
  *
  * ********************************************************
@@ -40,7 +40,8 @@ class Node
    * @param pid node's parent's id
    */
   Node(int x = 0, int y = 0, double g = 0.0, double h = 0.0, int id = 0, int pid = 0);
-
+  Node(const Node& n);
+
   /**
    * @brief Overloading operator + for Node class
    * @param n another Node
@@ -204,6 +205,4 @@ class LNode : public Node
   std::multimap<double, LNode*>::iterator open_it;  // iterator
 };
 
-
-
 #endif  // NODES_H

src/core/utils/src/nodes.cpp

@@ -28,7 +28,9 @@
 Node::Node(int x, int y, double g, double h, int id, int pid) : x_(x), y_(y), g_(g), h_(h), id_(id), pid_(pid)
 {
 }
-
+Node::Node(const Node& n) : x_(n.x_), y_(n.y_), g_(n.g_), h_(n.h_), id_(n.id_), pid_(n.pid_)
+{
+}
 /**
  * @brief Overloading operator + for Node class
  * @param n another Node