merge common functions into localPlanner

src/core/local_planner/apf_planner/include/apf_planner.h

@@ -102,12 +102,12 @@ class APFPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne
   bool initialized_;                       // initialized flag
   bool goal_reached_;                      // goal reached flag
   tf2_ros::Buffer* tf_;                    // transform buffer
-  costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
+  // costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
   unsigned char* local_costmap_;           // costmap char map
   nav_msgs::OccupancyGrid potential_map_;  // local potential field map
 
   int plan_index_;
-  std::vector<geometry_msgs::PoseStamped> global_plan_;
+  // std::vector<geometry_msgs::PoseStamped> global_plan_;
   geometry_msgs::PoseStamped target_ps_, current_ps_;
 
   double p_window_;     // next point distance

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

@@ -26,7 +26,7 @@ namespace apf_planner
  * @brief Construct a new APFPlanner object
  */
 APFPlanner::APFPlanner()
-  : initialized_(false), tf_(nullptr), costmap_ros_(nullptr), goal_reached_(false), plan_index_(0)
+  : initialized_(false), tf_(nullptr), goal_reached_(false), plan_index_(0)  //, costmap_ros_(nullptr)
 {
 }
 

src/core/local_planner/local_planner/include/local_planner.h

@@ -4,12 +4,12 @@
  * @file: local_planner.h
  * @brief: Contains the abstract local planner class
  * @author: Yang Haodong
- * @date: 2023-10-02
- * @version: 1.2
+ * @date: 2024-01-20
+ * @version: 1.3
  *
- * Copyright (c) 2024, Yang Haodong. 
+ * Copyright (c) 2024, Yang Haodong.
  * All rights reserved.
- * 
+ *
  * --------------------------------------------------------
  *
  * ********************************************************
@@ -154,6 +154,32 @@ class LocalPlanner
    */
   bool worldToMap(double wx, double wy, int& mx, int& my);
 
+  /**
+   * @brief Prune the path, removing the waypoints that the robot has already passed and distant waypoints
+   * @param robot_pose_global the robot's pose  [global]
+   * @return pruned path
+   */
+  std::vector<geometry_msgs::PoseStamped> prune(const geometry_msgs::PoseStamped robot_pose_global);
+
+  /**
+   * @brief Calculate the look-ahead distance with current speed dynamically
+   * @param vt the current speed
+   * @return L the look-ahead distance
+   */
+  double getLookAheadDistance(double vt);
+
+  /**
+   * @brief find the point on the path that is exactly the lookahead distance away from the robot
+   * @param lookahead_dist    the lookahead distance
+   * @param robot_pose_global the robot's pose  [global]
+   * @param prune_plan        the pruned plan
+   * @param pt                the lookahead point
+   * @param theta             the angle on traj
+   */
+  void getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseStamped robot_pose_global,
+                                       const std::vector<geometry_msgs::PoseStamped>& prune_plan,
+                                       geometry_msgs::PointStamped& pt, double& theta);
+
 protected:
   // lethal cost and neutral cost
   unsigned char lethal_cost_, neutral_cost_;
@@ -178,6 +204,13 @@ class LocalPlanner
 
   // odometry helper
   base_local_planner::OdometryHelperRos* odom_helper_;
+
+  costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
+  std::vector<geometry_msgs::PoseStamped> global_plan_;
+
+  double lookahead_time_;      // lookahead time gain
+  double min_lookahead_dist_;  // minimum lookahead distance
+  double max_lookahead_dist_;  // maximum lookahead distance
 };
 }  // namespace local_planner
 

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

@@ -4,8 +4,8 @@
  * @file: local_planner.cpp
  * @brief: Contains the abstract local planner class
  * @author: Yang Haodong
- * @date: 2023-10-02
- * @version: 1.2
+ * @date: 2024-01-20
+ * @version: 1.3
  *
  * Copyright (c) 2024, Yang Haodong.
  * All rights reserved.
@@ -31,6 +31,7 @@ LocalPlanner::LocalPlanner()
   , map_frame_("map")
   , odom_frame_("odom")
   , convert_offset_(0.0)
+  , costmap_ros_(nullptr)
 {
   odom_helper_ = new base_local_planner::OdometryHelperRos(odom_frame_);
 }
@@ -247,4 +248,106 @@ bool LocalPlanner::worldToMap(double wx, double wy, int& mx, int& my)
 
   return false;
 }
+
+/**
+ * @brief Prune the path, removing the waypoints that the robot has already passed and distant waypoints
+ * @param robot_pose_global the robot's pose  [global]
+ * @return pruned path
+ */
+std::vector<geometry_msgs::PoseStamped> LocalPlanner::prune(const geometry_msgs::PoseStamped robot_pose_global)
+{
+  auto calPoseDistance = [](const geometry_msgs::PoseStamped& ps_1, const geometry_msgs::PoseStamped& ps_2) {
+    return helper::dist(ps_1, ps_2);
+  };
+
+  auto closest_pose_upper_bound = helper::firstIntegratedDistance(
+      global_plan_.begin(), global_plan_.end(), calPoseDistance, costmap_ros_->getCostmap()->getSizeInMetersX() / 2.0);
+
+  // find the closest pose on the path to the robot
+  auto transform_begin =
+      helper::getMinFuncVal(global_plan_.begin(), closest_pose_upper_bound, [&](const geometry_msgs::PoseStamped& ps) {
+        return calPoseDistance(robot_pose_global, ps);
+      });
+
+  // Transform the near part of the global plan into the robot's frame of reference.
+  std::vector<geometry_msgs::PoseStamped> prune_path;
+  for (auto it = transform_begin; it < global_plan_.end(); it++)
+    prune_path.push_back(*it);
+
+  // path pruning: remove the portion of the global plan that already passed so don't process it on the next iteration
+  global_plan_.erase(std::begin(global_plan_), transform_begin);
+
+  return prune_path;
+}
+
+/**
+ * @brief Calculate the look-ahead distance with current speed dynamically
+ * @param vt the current speed
+ * @return L the look-ahead distance
+ */
+double LocalPlanner::getLookAheadDistance(double vt)
+{
+  double lookahead_dist = fabs(vt) * lookahead_time_;
+  return helper::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_);
+}
+
+/**
+ * @brief find the point on the path that is exactly the lookahead distance away from the robot
+ * @param lookahead_dist    the lookahead distance
+ * @param robot_pose_global the robot's pose  [global]
+ * @param prune_plan        the pruned plan
+ * @param pt                the lookahead point
+ * @param theta             the angle on traj
+ */
+void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseStamped robot_pose_global,
+                                     const std::vector<geometry_msgs::PoseStamped>& prune_plan,
+                                     geometry_msgs::PointStamped& pt, double& theta)
+{
+  double rx = robot_pose_global.pose.position.x;
+  double ry = robot_pose_global.pose.position.y;
+
+  // Find the first pose which is at a distance greater than the lookahead distance
+  auto goal_pose_it = std::find_if(prune_plan.begin(), prune_plan.end(), [&](const auto& ps) {
+    return helper::dist(ps, robot_pose_global) >= lookahead_dist;
+  });
+
+  std::vector<geometry_msgs::PoseStamped>::const_iterator prev_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);
+    pt.point.x = goal_pose_it->pose.position.x;
+    pt.point.y = goal_pose_it->pose.position.y;
+  }
+  else
+  {
+    // find the point on the line segment between the two poses
+    // that is exactly the lookahead distance away from the robot pose (the origin)
+    // 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);
+
+    double px = prev_pose_it->pose.position.x;
+    double py = prev_pose_it->pose.position.y;
+    double gx = goal_pose_it->pose.position.x;
+    double gy = goal_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);
+    std::pair<double, double> goal_p(gx - rx, gy - ry);
+    std::vector<std::pair<double, double>> i_points = helper::circleSegmentIntersection(prev_p, goal_p, lookahead_dist);
+
+    pt.point.x = i_points[0].first + rx;
+    pt.point.y = i_points[0].second + ry;
+  }
+
+  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/include/lqr_planner.h

@@ -73,31 +73,6 @@ class LQRPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne
    */
   bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
 
-protected:
-  /**
-   * @brief calculate the look-ahead distance with current speed dynamically
-   * @param vt the current speed
-   * @return L the look-ahead distance
-   */
-  double _getLookAheadDistance(double vt);
-
-  /**
-   * @brief find the point on the path that is exactly the lookahead distance away from the robot
-   * @param lookahead_dist    the lookahead distance
-   * @param robot_pose_global the robot's pose  [global]
-   * @param prune_plan        the pruned plan
-   * @return point            the lookahead point
-   */
-  geometry_msgs::PointStamped _getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseStamped robot_pose_global,
-                                                 const std::vector<geometry_msgs::PoseStamped>& prune_plan);
-
-  /**
-   * @brief Prune the path, removing the waypoints that the robot has already passed and distant waypoints
-   * @param robot_pose_global the robot's pose  [global]
-   * @return pruned path
-   */
-  std::vector<geometry_msgs::PoseStamped> _prune(const geometry_msgs::PoseStamped robot_pose_global);
-
 private:
   /**
    * @brief Execute LQR control process
@@ -107,24 +82,19 @@ class LQRPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne
    */
   Eigen::Vector2d _lqrControl(Eigen::Vector3d x, std::vector<double> ref);
 
-protected:
-  double lookahead_time_;      // lookahead time gain
-  double min_lookahead_dist_;  // minimum lookahead distance
-  double max_lookahead_dist_;  // maximum lookahead distance
-
 private:
   bool initialized_;                       // initialized flag
   bool goal_reached_;                      // goal reached flag
   tf2_ros::Buffer* tf_;                    // transform buffer
-  costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
+  // costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
 
   double d_t_;         // control time interval
   Eigen::Matrix3d Q_;  // state error matrix
   Eigen::Matrix2d R_;  // control error matrix
   int max_iter_;       // maximum iteration for ricatti solution
   double eps_iter_;    // iteration ending threshold
 
-  std::vector<geometry_msgs::PoseStamped> global_plan_;
+  // std::vector<geometry_msgs::PoseStamped> global_plan_;
   ros::Publisher target_pt_pub_, current_pose_pub_;
 
   // goal parameters