fix(control_performance_analysis): fix choosing wrong closest traject…

…ory point (autowarefoundation#1522) * fix(control_performance_analysis): fix choosing wrong closest trajectory point Signed-off-by: Berkay <[email protected]> * removed comments Signed-off-by: Berkay <[email protected]> * ci(pre-commit): autofix * Single-parameter constructors marked explicit. Signed-off-by: Berkay <[email protected]> * Removed unnecessary variable and change the struct name Signed-off-by: Berkay <[email protected]> * optimized declaration of variables Signed-off-by: Berkay <[email protected]> * update for parameter declaration Signed-off-by: Berkay <[email protected]> * ci(pre-commit): autofix * changed desired steering angle calculation Signed-off-by: Berkay <[email protected]> * ci(pre-commit): autofix * get reference of some variables Signed-off-by: Berkay <[email protected]> * ci(pre-commit): autofix Co-authored-by: Berkay <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Interplai · Aug 8, 2022 · 73c6e47 · 73c6e47
1 parent 3dbcae2
commit 73c6e47
Show file tree

Hide file tree

Showing 8 changed files with 294 additions and 359 deletions.
diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md
@@ -73,12 +73,14 @@ Error acceleration calculations are made based on the velocity calculations abov
 
 ## Parameters
 
-| Name                               | Type             | Description                                                     |
-| ---------------------------------- | ---------------- | --------------------------------------------------------------- |
-| `curvature_interval_length`        | double           | Used for estimating current curvature                           |
-| `prevent_zero_division_value`      | double           | Value to avoid zero division. Default is `0.001`                |
-| `odom_interval`                    | unsigned integer | Interval between odom messages, increase it for smoother curve. |
-| `acceptable_min_waypoint_distance` | double           | How far the next waypoint can be ahead of the vehicle direction |
+| Name                                  | Type             | Description                                                       |
+| ------------------------------------- | ---------------- | ----------------------------------------------------------------- |
+| `curvature_interval_length`           | double           | Used for estimating current curvature                             |
+| `prevent_zero_division_value`         | double           | Value to avoid zero division. Default is `0.001`                  |
+| `odom_interval`                       | unsigned integer | Interval between odom messages, increase it for smoother curve.   |
+| `acceptable_max_distance_to_waypoint` | double           | Maximum distance between trajectory point and vehicle [m]         |
+| `acceptable_max_yaw_difference_rad`   | double           | Maximum yaw difference between trajectory point and vehicle [rad] |
+| `low_pass_filter_gain`                | double           | Low pass filter gain                                              |
 
 ## Usage
 
@@ -93,3 +95,7 @@ Error acceleration calculations are made based on the velocity calculations abov
 > - /control_performance/performance_vars
 
 - In `Plotjuggler` you can export the statistic (max, min, average) values as csv file. Use that statistics to compare the control modules.
+
+## Future Improvements
+
+- Implement a LPF by cut-off frequency, differential equation and discrete state space update.
diff --git a/control/control_performance_analysis/config/control_performance_analysis.param.yaml b/control/control_performance_analysis/config/control_performance_analysis.param.yaml
@@ -1,8 +1,9 @@
 /**:
   ros__parameters:
     # -- publishing period --
-    curvature_interval_length_: 5.0
+    curvature_interval_length: 5.0
     prevent_zero_division_value: 0.001
-    odom_interval_: 1
-    acceptable_min_waypoint_distance: 2.0
+    odom_interval: 0
+    acceptable_max_distance_to_waypoint: 2.0
     low_pass_filter_gain: 0.95
+    acceptable_max_yaw_difference_rad: 1.0472
diff --git a/...mance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/...mance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp
@@ -19,6 +19,8 @@
 #include "control_performance_analysis/msg/driving_monitor_stamped.hpp"
 #include "control_performance_analysis/msg/error_stamped.hpp"
 #include "control_performance_analysis/msg/float_stamped.hpp"
+#include "motion_common/motion_common.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
 
 #include <eigen3/Eigen/Core>
 #include <rclcpp/time.hpp>
@@ -48,24 +50,32 @@ using geometry_msgs::msg::PoseArray;
 using geometry_msgs::msg::Twist;
 using nav_msgs::msg::Odometry;
 
+struct Params
+{
+  double wheelbase_;
+  double curvature_interval_length_;
+  uint odom_interval_;
+  double acceptable_max_distance_to_waypoint_;
+  double acceptable_max_yaw_difference_rad_;
+  double prevent_zero_division_value_;
+  double lpf_gain_;
+};
+
 class ControlPerformanceAnalysisCore
 {
 public:
   // See https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   ControlPerformanceAnalysisCore();
-  ControlPerformanceAnalysisCore(
-    double wheelbase, double curvature_interval_length, uint odom_interval,
-    double acceptable_min_waypoint_distance, double prevent_zero_division_value,
-    double lpf_gain_val);
+  explicit ControlPerformanceAnalysisCore(Params & p);
 
   // Setters
   void setCurrentPose(const Pose & msg);
   void setCurrentWaypoints(const Trajectory & trajectory);
   void setCurrentControlValue(const AckermannControlCommand & msg);
   void setInterpolatedVars(
-    Pose & interpolated_pose, double & interpolated_velocity, double & interpolated_acceleration,
-    double & interpolated_steering_angle);
+    const Pose & interpolated_pose, const double & interpolated_velocity,
+    const double & interpolated_acceleration, const double & interpolated_steering_angle);
   void setOdomHistory(const Odometry & odom);
   void setSteeringStatus(const SteeringReport & steering);
 
@@ -86,12 +96,7 @@ class ControlPerformanceAnalysisCore
   DrivingMonitorStamped driving_status_vars;
 
 private:
-  double wheelbase_;
-  double curvature_interval_length_;
-  uint odom_interval_;
-  double acceptable_min_waypoint_distance_;
-  double prevent_zero_division_value_;
-  double lpf_gain_;
+  Params p_;
 
   // Variables Received Outside
   std::shared_ptr<PoseArray> current_waypoints_ptr_;

diff --git a/...mance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/...mance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp
@@ -44,21 +44,6 @@ using control_performance_analysis::msg::ErrorStamped;
 using geometry_msgs::msg::PoseStamped;
 using nav_msgs::msg::Odometry;
 
-// Parameters Struct
-struct Param
-{
-  // Global parameters
-  double wheel_base;
-  double curvature_interval_length;
-  double prevent_zero_division_value;
-  uint odom_interval;  // Increase it for smoother curve
-  double lpf_gain;
-
-  // How far the next waypoint can be ahead of the vehicle direction.
-
-  double acceptable_min_waypoint_distance;
-};
-
 class ControlPerformanceAnalysisNode : public rclcpp::Node
 {
 public:
@@ -91,7 +76,7 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node
   void onVelocity(const Odometry::ConstSharedPtr msg);
 
   // Parameters
-  Param param_{};  // wheelbase, control period and feedback coefficients.
+  Params param_{};  // wheelbase, control period and feedback coefficients.
   // State holder
   std_msgs::msg::Header last_control_cmd_;
   double d_control_cmd_{0};
@@ -108,8 +93,6 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node
   AckermannControlCommand prev_cmd;
   SteeringReport prev_steering;
 
-  std_msgs::msg::Header odom_state_;
-
   // Algorithm
   std::unique_ptr<ControlPerformanceAnalysisCore> control_performance_core_ptr_;
 };

diff --git a/...ance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp b/...ance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp
@@ -42,8 +42,8 @@ inline std::vector<double> getNormalVector(double yaw_angle)
 }
 
 inline std::vector<double> computeLateralLongitudinalError(
-  std::vector<double> & closest_point_position, std::vector<double> & vehicle_position,
-  double & desired_yaw_angle)
+  const std::vector<double> & closest_point_position, const std::vector<double> & vehicle_position,
+  const double & desired_yaw_angle)
 {
   // Vector to path point originating from the vehicle r - rd
   std::vector<double> vector_to_path_point{
@@ -81,7 +81,7 @@ inline double computeLateralError(
  *  angles must be  carried out using the distance value instead of using the end values of
  *  two points.
  * */
-inline double angleDistance(double & target_angle, double const & reference_angle)
+inline double angleDistance(const double & target_angle, const double & reference_angle)
 {
   double diff = std::fmod(target_angle - reference_angle + M_PI_2, 2 * M_PI) - M_PI_2;
   double diff_signed_correction = diff < -M_PI_2 ? diff + 2 * M_PI : diff;  // Fix sign

diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml
@@ -20,6 +20,8 @@
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>libboost-dev</depend>
+  <depend>motion_common</depend>
+  <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>