update: regulated pure pursuit and closed ai-winter#13

README.md

@@ -154,6 +154,7 @@ For the efficient operation of the motion planning system, we provide a series o
 |   **PID**   | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/pid_planner/src/pid_planner.cpp) |           ![pid_ros.gif](assets/pid_ros.gif)            |
 |   **DWA**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/dwa_planner/src/dwa.cpp)     |           ![dwa_ros.gif](assets/dwa_ros.gif)            |
 |   **APF**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/apf_planner/src/apf_planner.cpp)     | ![apf_ros.gif](assets/apf_ros.gif)|
+|   **RPP**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/rpp_planner/src/rpp_planner.cpp)     | ![rpp_ros.gif](assets/rpp_ros.gif)|
 |   **TEB**   |                                                                ![Status](https://img.shields.io/badge/develop-v1.0-red)                                                                 | ![Status](https://img.shields.io/badge/gif-none-yellow) |
 |   **MPC**   |                                                                ![Status](https://img.shields.io/badge/develop-v1.0-red)                                                                 | ![Status](https://img.shields.io/badge/gif-none-yellow) |
 | **Lattice** |                                                                ![Status](https://img.shields.io/badge/develop-v1.0-red)                                                                 | ![Status](https://img.shields.io/badge/gif-none-yellow) |
@@ -207,6 +208,7 @@ For the efficient operation of the motion planning system, we provide a series o
 
 * DWA: [The Dynamic Window Approach to Collision Avoidance](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf).
 * APF: [Real-time obstacle avoidance for manipulators and mobile robots](https://ieeexplore.ieee.org/document/1087247).
+* RPP: [Regulated Pure Pursuit for Robot Path Tracking](https://arxiv.org/pdf/2305.20026.pdf)
 
 
 ### Curve Generation

src/core/curve_generation/include/dubins_curve.h

@@ -164,6 +164,9 @@ class Dubins : public Curve
 protected:
   double max_curv_;  // The maximum curvature of the curve
 };
+
+typedef void (Dubins::*DubinsSolver)(double, double, double, DubinsLength&, DubinsMode&);
+extern DubinsSolver dubins_solvers[];
 }  // namespace trajectory_generation
 
 #endif

src/core/curve_generation/src/dubins_curve.cpp

@@ -18,6 +18,8 @@
 
 namespace trajectory_generation
 {
+DubinsSolver dubins_solvers[] = { &Dubins::LRL, &Dubins::LSL, &Dubins::LSR, &Dubins::RLR, &Dubins::RSL, &Dubins::RSR };
+
 /**
  * @brief Construct a new Dubins generation object
  * @param step        Simulation or interpolation size (default: 0.1)
@@ -271,18 +273,12 @@ Points2d Dubins::generation(Pose2d start, Pose2d goal)
   DubinsLength best_length = { DUBINS_NONE, DUBINS_NONE, DUBINS_NONE };
   DubinsMode mode;
 
-  LSL(alpha, beta, dist, length, mode);
-  _update(length, mode, best_length, best_mode, best_cost);
-  RSR(alpha, beta, dist, length, mode);
-  _update(length, mode, best_length, best_mode, best_cost);
-  LSR(alpha, beta, dist, length, mode);
-  _update(length, mode, best_length, best_mode, best_cost);
-  RSL(alpha, beta, dist, length, mode);
-  _update(length, mode, best_length, best_mode, best_cost);
-  RLR(alpha, beta, dist, length, mode);
-  _update(length, mode, best_length, best_mode, best_cost);
-  LRL(alpha, beta, dist, length, mode);
-  _update(length, mode, best_length, best_mode, best_cost);
+  for (const auto solver : dubins_solvers)
+  {
+    (this->*solver)(alpha, beta, dist, length, mode);
+    _update(length, mode, best_length, best_mode, best_cost);
+  }
+
   if (best_cost == DUBINS_MAX)
     return path;
 

src/core/global_planner/evolutionary_planner/src/aco.cpp

@@ -12,7 +12,6 @@
  *
  **********************************************************/
 #include <algorithm>
-#include <iostream>
 #include "aco.h"
 
 namespace global_planner

src/core/global_planner/evolutionary_planner/src/evolutionary_planner.cpp

@@ -343,7 +343,6 @@ bool EvolutionaryPlanner::_getPlanFromPath(std::vector<Node>& path, std::vector<
     ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
     return false;
   }
-  std::string globalFrame = frame_id_;
   ros::Time planTime = ros::Time::now();
   plan.clear();
 

src/core/global_planner/graph_planner/src/graph_planner.cpp

@@ -394,7 +394,6 @@ bool GraphPlanner::_getPlanFromPath(std::vector<Node>& path, std::vector<geometr
     ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
     return false;
   }
-  std::string globalFrame = frame_id_;
   ros::Time planTime = ros::Time::now();
   plan.clear();
 

src/core/global_planner/graph_planner/src/theta_star.cpp

@@ -12,7 +12,6 @@
  *
  **********************************************************/
 #include "theta_star.h"
-#include <iostream>
 
 namespace global_planner
 {

src/core/global_planner/graph_planner/src/voronoi.cpp

@@ -15,7 +15,6 @@
 #include <queue>
 #include <cmath>
 #include <unordered_set>
-#include <iostream>
 
 #include "voronoi.h"
 

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

@@ -15,20 +15,15 @@
 #ifndef APF_PLANNER_H_
 #define APF_PLANNER_H_
 
-#include <ros/ros.h>
-#include <nav_core/base_local_planner.h>
 #include <tf2_ros/buffer.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Twist.h>
 
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/OccupancyGrid.h>
-#include <base_local_planner/odometry_helper_ros.h>
 #include <tf2/utils.h>
 
-#include <Eigen/Dense>
-
 #include "local_planner.h"
 
 namespace apf_planner
@@ -82,22 +77,6 @@ class APFPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne
    */
   bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
 
-  /**
-   * @brief APF controller in linear
-   * @param base_odometry odometry of the robot, to get velocity
-   * @param v_d           desired velocity magnitude
-   * @return  linear velocity
-   */
-  double LinearAPFController(nav_msgs::Odometry& base_odometry, double v_d);
-
-  /**
-   * @brief APF controller in angular
-   * @param base_odometry odometry of the robot, to get velocity
-   * @param e_theta       the error between the current and desired theta
-   * @return  angular velocity
-   */
-  double AngularAPFController(nav_msgs::Odometry& base_odometry, double e_theta);
-
   /**
    * @brief Get the attractive force of APF
    * @param ps      global target PoseStamped
@@ -118,37 +97,35 @@ class APFPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne
   void publishPotentialMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
 
 private:
-  bool initialized_, goal_reached_;
-  tf2_ros::Buffer* tf_;
+  bool initialized_;                       // initialized flag
+  bool goal_reached_;                      // goal reached flag
+  tf2_ros::Buffer* tf_;                    // transform buffer
   costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
   unsigned char* local_costmap_;           // costmap char map
-  nav_msgs::OccupancyGrid potential_map_; // local potential field map
+  nav_msgs::OccupancyGrid potential_map_;  // local potential field map
 
   int plan_index_;
   std::vector<geometry_msgs::PoseStamped> global_plan_;
   geometry_msgs::PoseStamped target_ps_, current_ps_;
 
-  double p_window_;                   // next point distance
-  double p_precision_, o_precision_;  // goal reached tolerance
-  double d_t_;                        // control time step
-
-  double max_v_, min_v_, max_v_inc_;  // linear velocity
-  double max_w_, min_w_, max_w_inc_;  // angular velocity
+  double p_window_;     // next point distance
+  double o_precision_;  // goal reached tolerance
+  double d_t_;          // control time step
 
   int s_window_;  // trajectory smoothing window time
 
   double zeta_, eta_;  // scale factor of attractive and repulsive force
 
   int cost_ub_, cost_lb_;  // the upper and lower bound of costmap used to calculate potential field
 
-  double inflation_radius_; // the costmap inflation radius of obstacles
+  double inflation_radius_;  // the costmap inflation radius of obstacles
 
   std::deque<Eigen::Vector2d> hist_nf_;  // historical net forces
 
-  base_local_planner::OdometryHelperRos* odom_helper_;
   ros::Publisher target_pose_pub_, current_pose_pub_, potential_map_pub_;
-  ros::Subscriber costmap_sub_;   // subscribe local map topic to generate potential field
+  ros::Subscriber costmap_sub_;  // subscribe local map topic to generate potential field
 
+  // goal parameters
   double goal_x_, goal_y_;
   Eigen::Vector3d goal_rpy_;
 };

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

@@ -11,9 +11,9 @@
  * --------------------------------------------------------
  *
  **********************************************************/
+#include <pluginlib/class_list_macros.h>
 
 #include "apf_planner.h"
-#include <pluginlib/class_list_macros.h>
 
 PLUGINLIB_EXPORT_CLASS(apf_planner::APFPlanner, nav_core::BaseLocalPlanner)
 
@@ -69,8 +69,8 @@ void APFPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::C
 
     nh.param("p_window", p_window_, 0.5);
 
-    nh.param("p_precision", p_precision_, 0.2);
-    nh.param("o_precision", o_precision_, 0.5);
+    nh.param("goal_dist_tolerance", goal_dist_tol_, 0.2);
+    nh.param("rotate_tolerance", rotate_tol_, 0.5);
 
     nh.param("max_v", max_v_, 0.5);
     nh.param("min_v", min_v_, 0.0);
@@ -99,7 +99,6 @@ void APFPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::C
 
     hist_nf_.clear();
 
-    odom_helper_ = new base_local_planner::OdometryHelperRos("/odom");
     target_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/target_pose", 10);
     current_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 10);
     potential_map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/potential_map", 10);
@@ -181,12 +180,10 @@ bool APFPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   nav_msgs::Odometry base_odom;
   odom_helper_->getOdom(base_odom);
 
-  // current pose
+  // get robot position in global frame
   geometry_msgs::PoseStamped current_ps_odom;
   costmap_ros_->getRobotPose(current_ps_odom);
-
-  // transform into map
-  tf_->transform(current_ps_odom, current_ps_, map_frame_);
+  transformPose(tf_, map_frame_, current_ps_odom, current_ps_);
 
   // current angle
   double theta = tf2::getYaw(current_ps_.pose.orientation);
@@ -240,14 +237,13 @@ bool APFPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   regularizeAngle(e_theta);
 
   // position reached
-  if (helper::dist(Eigen::Vector2d(global_plan_.back().pose.position.x, global_plan_.back().pose.position.y),
-                   Eigen::Vector2d(current_ps_.pose.position.x, current_ps_.pose.position.y)) < p_precision_)
+  if (shouldRotateToGoal(current_ps_, global_plan_.back()))
   {
     e_theta = goal_rpy_.z() - theta;
     regularizeAngle(e_theta);
 
     // orientation reached
-    if (std::fabs(e_theta) < o_precision_)
+    if (!shouldRotateToPath(std::fabs(e_theta)))
     {
       cmd_vel.linear.x = 0.0;
       cmd_vel.angular.z = 0.0;
@@ -257,20 +253,20 @@ bool APFPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
     else
     {
       cmd_vel.linear.x = 0.0;
-      cmd_vel.angular.z = AngularAPFController(base_odom, e_theta);
+      cmd_vel.angular.z = angularRegularization(base_odom, e_theta / d_t_);
     }
   }
   // large angle, turn first
-  else if (std::fabs(e_theta) > M_PI_2)
+  else if (shouldRotateToPath(std::fabs(e_theta), M_PI_2))
   {
     cmd_vel.linear.x = 0.0;
-    cmd_vel.angular.z = AngularAPFController(base_odom, e_theta);
+    cmd_vel.angular.z = angularRegularization(base_odom, e_theta / d_t_);
   }
   // posistion not reached
   else
   {
-    cmd_vel.linear.x = LinearAPFController(base_odom, new_v.norm());
-    cmd_vel.angular.z = AngularAPFController(base_odom, e_theta);
+    cmd_vel.linear.x = linearRegularization(base_odom, new_v.norm());
+    cmd_vel.angular.z = angularRegularization(base_odom, e_theta / d_t_);
   }
 
   // publish next target_ps_ pose
@@ -282,59 +278,6 @@ bool APFPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   return true;
 }
 
-/**
- * @brief APF controller in linear
- * @param base_odometry odometry of the robot, to get velocity
- * @param b_x_d         desired x in body frame
- * @param b_y_d         desired y in body frame
- * @return  linear velocity
- */
-double APFPlanner::LinearAPFController(nav_msgs::Odometry& base_odometry, double v_d)
-{
-  double v = std::hypot(base_odometry.twist.twist.linear.x, base_odometry.twist.twist.linear.y);
-  double v_inc = v_d - v;
-
-  if (std::fabs(v_inc) > max_v_inc_)
-    v_inc = std::copysign(max_v_inc_, v_inc);
-
-  double v_cmd = v + v_inc;
-  if (std::fabs(v_cmd) > max_v_)
-    v_cmd = std::copysign(max_v_, v_cmd);
-  else if (std::fabs(v_cmd) < min_v_)
-    v_cmd = std::copysign(min_v_, v_cmd);
-
-  return v_cmd;
-}
-
-/**
- * @brief APF controller in angular
- * @param base_odometry odometry of the robot, to get velocity
- * @param e_theta       the error between the current and desired theta
- * @return  angular velocity
- */
-double APFPlanner::AngularAPFController(nav_msgs::Odometry& base_odometry, double e_theta)
-{
-  regularizeAngle(e_theta);
-
-  double w_d = e_theta / d_t_;
-  if (std::fabs(w_d) > max_w_)
-    w_d = std::copysign(max_w_, w_d);
-
-  double w = base_odometry.twist.twist.angular.z;
-  double w_inc = w_d - w;
-
-  if (std::fabs(w_inc) > max_w_inc_)
-    w_inc = std::copysign(max_w_inc_, w_inc);
-
-  double w_cmd = w + w_inc;
-  if (std::fabs(w_cmd) > max_w_)
-    w_cmd = std::copysign(max_w_, w_cmd);
-  else if (std::fabs(w_cmd) < min_w_)
-    w_cmd = std::copysign(min_w_, w_cmd);
-
-  return w_cmd;
-}
-
 /**
  * @brief Get the attractive force of APF
  * @param ps      global target PoseStamped