update: PID

Bnedict · Oct 2, 2023 · 2807c82 · 2807c82
1 parent 8626cd3
commit 2807c82
Show file tree

Hide file tree

Showing 10 changed files with 98 additions and 211 deletions.
diff --git a/README.md b/README.md
@@ -278,7 +278,7 @@ Explanation:
 |:-----------:|:---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------:| :-----------------------------------------------------: |
 |   **PID**   | [![Status](https://img.shields.io/badge/done-v1.1-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/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/planner/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/planner/local_planner/apf_planner/src/apf.cpp)     | ![Status](https://img.shields.io/badge/gif-none-yellow) |
+|   **APF**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/planner/local_planner/apf_planner/src/apf_planner.cpp)     | ![apf_ros.gif](assets/apf_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) |

diff --git a/assets/apf_ros.gif b/assets/apf_ros.gif
diff --git a/src/planner/local_planner/apf_planner/include/apf_planner.h b/src/planner/local_planner/apf_planner/include/apf_planner.h
@@ -100,35 +100,29 @@ class APFPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne
   /**
    * @brief Get the attractive force of APF
    * @param ps      global target PoseStamped
-   * @param x       global current x
-   * @param y       global current y
    * @return the attractive force
    */
-  Eigen::Vector2d getAttractiveForce(const geometry_msgs::PoseStamped& ps, double x, double y);
+  Eigen::Vector2d getAttractiveForce(const geometry_msgs::PoseStamped& ps);
 
   /**
    * @brief Get the repulsive force of APF
    * @return the repulsive force
    */
   Eigen::Vector2d getRepulsiveForce();
 
-
 private:
   bool initialized_, goal_reached_;
   tf2_ros::Buffer* tf_;
   costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
-  costmap_2d::Costmap2D* costmap_;         // costmap
-  unsigned char* costmap_char_;            // costmap char map
+  unsigned char* local_costmap_;           // costmap char map
 
   int plan_index_;
   std::vector<geometry_msgs::PoseStamped> global_plan_;
   geometry_msgs::PoseStamped target_ps_, current_ps_;
 
-  double x_, y_, theta_;
-
   double p_window_;                   // next point distance
   double p_precision_, o_precision_;  // goal reached tolerance
-  double controller_freqency_, d_t_;
+  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

diff --git a/src/planner/local_planner/apf_planner/src/apf_planner.cpp b/src/planner/local_planner/apf_planner/src/apf_planner.cpp
@@ -55,13 +55,13 @@ void APFPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::C
     initialized_ = true;
     tf_ = tf;
     costmap_ros_ = costmap_ros;
-    costmap_ = costmap_ros_->getCostmap();
-    costmap_char_ = costmap_->getCharMap();
+    costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();
+    local_costmap_ = costmap->getCharMap();
 
     // set costmap properties
-    setSize(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
-    setOrigin(costmap_->getOriginX(), costmap_->getOriginY());
-    setResolution(costmap_->getResolution());
+    setSize(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
+    setOrigin(costmap->getOriginX(), costmap->getOriginY());
+    setResolution(costmap->getResolution());
 
     ros::NodeHandle nh = ros::NodeHandle("~/" + name);
 
@@ -91,8 +91,9 @@ void APFPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::C
     nh.param("base_frame", base_frame_, base_frame_);
     nh.param("map_frame", map_frame_, map_frame_);
 
-    nh.param("/move_base/controller_frequency", controller_freqency_, 10.0);
-    d_t_ = 1 / controller_freqency_;
+    double controller_freqency;
+    nh.param("/move_base/controller_frequency", controller_freqency, 10.0);
+    d_t_ = 1 / controller_freqency;
 
     hist_nf_.clear();
 
@@ -182,17 +183,16 @@ bool APFPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   // transform into map
   tf_->transform(current_ps_odom, current_ps_, map_frame_);
 
-  x_ = current_ps_.pose.position.x;
-  y_ = current_ps_.pose.position.y;
-  theta_ = tf2::getYaw(current_ps_.pose.orientation);  // [-pi, pi]
+  // current angle
+  double theta = tf2::getYaw(current_ps_.pose.orientation);
 
   // compute the tatget pose and force at the current step
   Eigen::Vector2d attr_force, rep_force, net_force;
   rep_force = getRepulsiveForce();
   while (plan_index_ < global_plan_.size())
   {
     target_ps_ = global_plan_[plan_index_];
-    attr_force = getAttractiveForce(target_ps_, x_, y_);
+    attr_force = getAttractiveForce(target_ps_);
     net_force = zeta_ * attr_force + eta_ * rep_force;
 
     // transform from map into base_frame
@@ -231,14 +231,14 @@ bool APFPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   tf2::Quaternion q;
   q.setRPY(0, 0, theta_d);
   tf2::convert(q, target_ps_.pose.orientation);
-  double e_theta = theta_d - theta_;
+  double e_theta = theta_d - theta;
   regularizeAngle(e_theta);
 
   // position reached
   if (dist(Eigen::Vector2d(global_plan_.back().pose.position.x, global_plan_.back().pose.position.y),
-           Eigen::Vector2d(x_, y_)) < p_precision_)
+           Eigen::Vector2d(current_ps_.pose.position.x, current_ps_.pose.position.y)) < p_precision_)
   {
-    e_theta = goal_rpy_.z() - theta_;
+    e_theta = goal_rpy_.z() - theta;
     regularizeAngle(e_theta);
 
     // orientation reached
@@ -333,12 +333,12 @@ double APFPlanner::AngularAPFController(nav_msgs::Odometry& base_odometry, doubl
 /**
  * @brief Get the attractive force of APF
  * @param ps      global target PoseStamped
- * @param x       global current x
- * @param y       global current y
  * @return the attractive force
  */
-Eigen::Vector2d APFPlanner::getAttractiveForce(const geometry_msgs::PoseStamped& ps, double x, double y)
+Eigen::Vector2d APFPlanner::getAttractiveForce(const geometry_msgs::PoseStamped& ps)
 {
+  double x = current_ps_.pose.position.x;
+  double y = current_ps_.pose.position.y;
   Eigen::Vector2d attr_force = Eigen::Vector2d(ps.pose.position.x - x, ps.pose.position.y - y);
   return attr_force / attr_force.norm();  // normalization
 }
@@ -357,7 +357,7 @@ Eigen::Vector2d APFPlanner::getRepulsiveForce()
     return rep_force;
   }
 
-  double current_cost = costmap_char_[mx + nx_ * my];
+  double current_cost = local_costmap_[mx + nx_ * my];
 
   if (current_cost >= cost_ub_ || current_cost < cost_lb_)
   {
@@ -373,10 +373,10 @@ Eigen::Vector2d APFPlanner::getRepulsiveForce()
   double bound_diff = cost_ub_ - cost_lb_;
   double dist = (cost_ub_ - current_cost) / bound_diff;
   double k = (1.0 - 1.0 / dist) / (dist * dist);
-  double next_x = costmap_char_[std::min(mx + 1, (int)nx_ - 1) + nx_ * my];
-  double prev_x = costmap_char_[std::max(mx - 1, 0) + nx_ * my];
-  double next_y = costmap_char_[mx + nx_ * std::min(my + 1, (int)ny_ - 1)];
-  double prev_y = costmap_char_[mx + nx_ * std::max(my - 1, 0)];
+  double next_x = local_costmap_[std::min(mx + 1, (int)nx_ - 1) + nx_ * my];
+  double prev_x = local_costmap_[std::max(mx - 1, 0) + nx_ * my];
+  double next_y = local_costmap_[mx + nx_ * std::min(my + 1, (int)ny_ - 1)];
+  double prev_y = local_costmap_[mx + nx_ * std::max(my - 1, 0)];
   Eigen::Vector2d grad_dist((next_x - prev_x) / (2.0 * bound_diff), (next_y - prev_y) / (2.0 * bound_diff));
 
   rep_force = k * grad_dist;

diff --git a/src/planner/local_planner/pid_planner/CMakeLists.txt b/src/planner/local_planner/pid_planner/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_geometry_msgs
   tf2_ros
   base_local_planner
+  local_planner
 )
 
 # uncomment the following 4 lines to use the Eigen library
@@ -36,4 +37,5 @@ add_library(${PROJECT_NAME}
 
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
+  local_planner
 )
diff --git a/src/planner/local_planner/pid_planner/include/pid_planner.h b/src/planner/local_planner/pid_planner/include/pid_planner.h
@@ -1,16 +1,16 @@
 /***********************************************************
-*
-* @file: pid_planner.h
-* @breif: Contains the Proportional–Integral–Derivative (PID) controller local planner class
-* @author: Yang Haodong, Wu Maojia
-* @update: 2023-10-1
-* @version: 1.1
-*
-* Copyright (c) 2023，Yang Haodong
-* All rights reserved.
-* --------------------------------------------------------
-*
-**********************************************************/
+ *
+ * @file: pid_planner.h
+ * @breif: Contains the Proportional–Integral–Derivative (PID) controller local planner class
+ * @author: Yang Haodong, Guo Zhanyu, Wu Maojia
+ * @update: 2023-10-1
+ * @version: 1.1
+ *
+ * Copyright (c) 2023，Yang Haodong
+ * All rights reserved.
+ * --------------------------------------------------------
+ *
+ **********************************************************/
 
 #ifndef PID_PLANNER_H_
 #define PID_PLANNER_H_
@@ -25,13 +25,16 @@
 #include <nav_msgs/Odometry.h>
 #include <base_local_planner/odometry_helper_ros.h>
 #include <tf2/utils.h>
+#include <Eigen/Dense>
+
+#include "local_planner.h"
 
 namespace pid_planner
 {
 /**
  * @brief A class implementing a local planner using the PID
  */
-class PIDPlanner : public nav_core::BaseLocalPlanner
+class PIDPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanner
 {
 public:
   /**
@@ -89,41 +92,10 @@ class PIDPlanner : public nav_core::BaseLocalPlanner
   /**
    * @brief PID controller in angular
    * @param base_odometry odometry of the robot, to get velocity
-   * @param theta_d       desired theta
-   * @param theta         current theta
+   * @param e_theta       the error between the current and desired theta
    * @return  angular velocity
    */
-  double AngularPIDController(nav_msgs::Odometry& base_odometry, double theta_d, double theta);
-
-  /**
-   * @brief Get the distance to the goal
-   * @param goal_ps global goal PoseStamped
-   * @param x       global current x
-   * @param y       global current y
-   * @return the distance to the goal
-   */
-  double getGoalPositionDistance(const geometry_msgs::PoseStamped& goal_ps, double x, double y);
-
-  /**
-   * @brief Get the Euler Angles from PoseStamped
-   * @param ps  PoseStamped to calculate
-   * @return  roll, pitch and yaw in XYZ order
-   */
-  std::vector<double> getEulerAngles(geometry_msgs::PoseStamped& ps);
-
-  /**
-   * @brief Transform pose to body frame
-   * @param src   src PoseStamped, the object to transform
-   * @param x     result x
-   * @param y     result y
-   */
-  void getTransformedPosition(geometry_msgs::PoseStamped& src, double& x, double& y);
-
-  /**
-   * @brief Regularize angle to [-pi, pi]
-   * @param angle the angle (rad) to regularize
-   */
-  void regularizeAngle(double& angle);
+  double AngularPIDController(nav_msgs::Odometry& base_odometry, double e_theta);
 
 private:
   bool initialized_, goal_reached_;
@@ -134,11 +106,9 @@ class PIDPlanner : public nav_core::BaseLocalPlanner
   std::vector<geometry_msgs::PoseStamped> global_plan_;
   geometry_msgs::PoseStamped target_ps_, current_ps_;
 
-  double x_, y_, theta_;
-
   double p_window_;                   // next point distance
   double p_precision_, o_precision_;  // goal reached tolerance
-  double controller_freqency_, d_t_;
+  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
@@ -150,12 +120,11 @@ class PIDPlanner : public nav_core::BaseLocalPlanner
   double e_v_, e_w_;
   double i_v_, i_w_;
 
-  std::string base_frame_, map_frame_;
   base_local_planner::OdometryHelperRos* odom_helper_;
   ros::Publisher target_pose_pub_, current_pose_pub_;
 
   double goal_x_, goal_y_;
-  std::vector<double> goal_rpy_;
+  Eigen::Vector3d goal_rpy_;
 };
 };  // namespace pid_planner
 

diff --git a/src/planner/local_planner/pid_planner/package.xml b/src/planner/local_planner/pid_planner/package.xml
@@ -1,42 +1,23 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>pid_planner</name>
-  <version>0.0.0</version>
+  <version>1.0.0</version>
   <description>The pid_planner package</description>
-  <maintainer email="[email protected]">gzy</maintainer>
+  <maintainer email="[email protected]">Yang Haodong</maintainer>
   <license>GPL3</license>
 
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>angles</build_depend>
-  <build_depend>costmap_2d</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>nav_core</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>navfn</build_depend>
-  <build_depend>pluginlib</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>tf2_geometry_msgs</build_depend>
-  <build_depend>tf2_ros</build_depend>
-  <build_export_depend>angles</build_export_depend>
-  <build_export_depend>costmap_2d</build_export_depend>
-  <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>nav_core</build_export_depend>
-  <build_export_depend>nav_msgs</build_export_depend>
-  <build_export_depend>navfn</build_export_depend>
-  <build_export_depend>pluginlib</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>tf2_geometry_msgs</build_export_depend>
-  <build_export_depend>tf2_ros</build_export_depend>
-  <exec_depend>angles</exec_depend>
-  <exec_depend>costmap_2d</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>nav_core</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>navfn</exec_depend>
-  <exec_depend>pluginlib</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>tf2_geometry_msgs</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
+  <depend>angles</depend>
+  <depend>costmap_2d</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_core</depend>
+  <depend>nav_msgs</depend>
+  <depend>navfn</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>local_planner</depend>
 
   <export>
     <nav_core plugin="${prefix}/pid_planner_plugin.xml" />