evaluation node updated(autowarefoundation#567)

message files arranged, structure changed(autowarefoundation#567)


ci(pre-commit): autofix

added another condition


added velocity history information to estimate acceleration


fix


new message file added monitor variables ready


fix


can build


runtime errors


works


works


fix

Signed-off-by: Berkay <berkay@leodrive.ai>

ci(pre-commit): autofix

odom history interval param added (autowarefoundation#567)

Signed-off-by: Berkay <berkay@leodrive.ai>

control/control_performance_analysis/CMakeLists.txt

@@ -8,14 +8,18 @@ find_package(Boost REQUIRED)
 
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
-
-rosidl_generate_interfaces(control_performance_analysis_msgs
-  msg/Error.msg
-  msg/ErrorStamped.msg
-  msg/DrivingMonitor.msg
-  msg/DrivingMonitorStamped.msg
-  DEPENDENCIES
-    std_msgs
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Error.msg"
+  "msg/ErrorStamped.msg"
+  "msg/DrivingMonitor.msg"
+  "msg/DrivingMonitorStamped.msg"
+  "msg/FloatStamped.msg"
+  DEPENDENCIES builtin_interfaces std_msgs
 )
 
 ament_auto_add_library(control_performance_analysis_core SHARED
@@ -27,17 +31,20 @@ ament_auto_add_library(control_performance_analysis_node SHARED
   src/control_performance_analysis_node.cpp
 )
 
-if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
-  rosidl_target_interfaces(control_performance_analysis_node
-    control_performance_analysis_msgs "rosidl_typesupport_cpp")
-else()
-  rosidl_get_typesupport_target(
-    cpp_typesupport_target control_performance_analysis_msgs "rosidl_typesupport_cpp")
-  target_link_libraries(control_performance_analysis_node "${cpp_typesupport_target}")
-endif()
+
+rclcpp_components_register_node(control_performance_analysis_node
+  PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode"
+  EXECUTABLE control_performance_analysis_exe
+)
+
+rosidl_target_interfaces(control_performance_analysis_core
+        ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+rosidl_target_interfaces(control_performance_analysis_node
+        ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
 target_link_libraries(control_performance_analysis_node
-  control_performance_analysis_core
+        control_performance_analysis_core
 )
 
 rclcpp_components_register_node(control_performance_analysis_node

control/control_performance_analysis/config/control_performance_analysis.param.yaml

@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
     # -- publishing period --
-    control_period: 0.033
-    double curvature_interval_length_: 5.0
+    control_period: 0.01
+    curvature_interval_length_: 5.0
+    odom_interval_: 3

control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp

@@ -16,54 +16,54 @@
 #define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_
 
 #include "control_performance_analysis/control_performance_analysis_utils.hpp"
+#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 <eigen3/Eigen/Core>
+#include <rclcpp/time.hpp>
 
-#include <autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp>
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 
 #include <memory>
 #include <utility>
 #include <vector>
 
 namespace control_performance_analysis
 {
-using autoware_auto_control_msgs::msg::AckermannLateralCommand;
+using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_vehicle_msgs::msg::SteeringReport;
+using control_performance_analysis::msg::DrivingMonitorStamped;
+using control_performance_analysis::msg::ErrorStamped;
+using control_performance_analysis::msg::FloatStamped;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::PoseArray;
 using geometry_msgs::msg::Twist;
-
-struct TargetPerformanceMsgVars
-{
-  double lateral_error;
-  double heading_error;
-  double control_effort_energy;
-  double error_energy;
-  double value_approximation;
-  double curvature_estimate;
-  double curvature_estimate_pp;
-  double lateral_error_velocity;
-  double lateral_error_acceleration;
-};
+using nav_msgs::msg::Odometry;
 
 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);
+  ControlPerformanceAnalysisCore(double wheelbase, double curvature_interval_length, uint odom_interval);
 
   // Setters
   void setCurrentPose(const Pose & msg);
   void setCurrentWaypoints(const Trajectory & trajectory);
   void setCurrentVelocities(const Twist & twist_msg);
-  void setCurrentControlValue(const AckermannLateralCommand & msg);
-  void setInterpolatedPose(Pose & interpolated_pose);
+  void setCurrentControlValue(const AckermannControlCommand & msg);
+  void setInterpolatedVars(Pose & interpolated_pose, double & interpolated_velocity);
+  void setOdomHistory(const Odometry & odom);
+  void setSteeringStatus(const SteeringReport & steering);
 
   void findCurveRefIdx();
   std::pair<bool, int32_t> findClosestPrevWayPointIdx_path_direction();
@@ -72,26 +72,44 @@ class ControlPerformanceAnalysisCore
 
   // Getters
   bool isDataReady() const;
-  std::pair<bool, TargetPerformanceMsgVars> getPerformanceVars();
+  bool calculateErrorVars();
+  bool calculateDrivingVars();
   Pose getPrevWPPose() const;
   std::pair<bool, Pose> calculateClosestPose();
 
+  // Output variables
+  ErrorStamped error_vars;
+  DrivingMonitorStamped driving_status_vars;
+
 private:
   double wheelbase_;
   double curvature_interval_length_;
+  uint odom_interval_;
 
   // Variables Received Outside
   std::shared_ptr<PoseArray> current_waypoints_ptr_;
+  std::shared_ptr<std::vector<double>> current_waypoints_vel_ptr_;
   std::shared_ptr<Pose> current_vec_pose_ptr_;
   std::shared_ptr<std::vector<double>> current_velocities_ptr_;  // [Vx, Heading rate]
-  std::shared_ptr<AckermannLateralCommand> current_control_ptr_;
+  std::shared_ptr<std::vector<Odometry>> odom_history_ptr_;      // velocities at t-2, t-1, t
+  std::shared_ptr<AckermannControlCommand> current_control_ptr_;
+  std::shared_ptr<SteeringReport> current_vec_steering_msg_ptr_;
+
+  // State holder
+
+  std_msgs::msg::Header last_odom_header;
+
 
   // Variables computed
+
   std::unique_ptr<int32_t> idx_prev_wp_;       // the waypoint index, vehicle
   std::unique_ptr<int32_t> idx_curve_ref_wp_;  // index of waypoint corresponds to front axle center
   std::unique_ptr<int32_t> idx_next_wp_;       //  the next waypoint index, vehicle heading to
-  std::unique_ptr<TargetPerformanceMsgVars> prev_target_vars_{};
+  std::unique_ptr<ErrorStamped> prev_target_vars_{};
+  std::unique_ptr<DrivingMonitorStamped> prev_driving_vars_{};
   std::shared_ptr<Pose> interpolated_pose_ptr_;
+  std::shared_ptr<double> interpolated_velocity_ptr_;
+
   // V = xPx' ; Value function from DARE Lyap matrix P
   Eigen::Matrix2d const lyap_P_ = (Eigen::MatrixXd(2, 2) << 2.342, 8.60, 8.60, 64.29).finished();
   double const contR{10.0};  // Control weight in LQR

control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp

@@ -16,12 +16,14 @@
 #define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_
 
 #include "control_performance_analysis/control_performance_analysis_core.hpp"
+#include "control_performance_analysis/msg/driving_monitor_stamped.hpp"
 #include "control_performance_analysis/msg/error_stamped.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <signal_processing/lowpass_filter_1d.hpp>
 #include <tier4_autoware_utils/ros/self_pose_listener.hpp>
 
-#include <autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp>
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -33,9 +35,10 @@
 
 namespace control_performance_analysis
 {
-using autoware_auto_control_msgs::msg::AckermannLateralCommand;
+using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_vehicle_msgs::msg::SteeringReport;
+using control_performance_analysis::msg::DrivingMonitorStamped;
 using control_performance_analysis::msg::ErrorStamped;
 using geometry_msgs::msg::PoseStamped;
 using nav_msgs::msg::Odometry;
@@ -49,18 +52,26 @@ struct Param
 
   // Control Method Parameters
   double control_period;
+
+  // LPF1d Gain
+
+  double lpf_gain;
+
+  uint odom_interval;
 };
 
 class ControlPerformanceAnalysisNode : public rclcpp::Node
 {
 public:
   explicit ControlPerformanceAnalysisNode(const rclcpp::NodeOptions & node_options);
+  // LPF
+  std::shared_ptr<LowpassFilter1d> filter_process_time_;
 
 private:
   // Subscribers and Local Variable Assignment
   rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;  // subscribe to trajectory
-  rclcpp::Subscription<AckermannLateralCommand>::SharedPtr
-    sub_control_steering_;                                  // subscribe to steering control value
+  rclcpp::Subscription<AckermannControlCommand>::SharedPtr
+    sub_control_cmd_;                                       // subscribe to steering control value
   rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_;  // subscribe to velocity
   rclcpp::Subscription<SteeringReport>::SharedPtr sub_vehicle_steering_;
 
@@ -69,16 +80,17 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node
 
   // Publishers
   rclcpp::Publisher<ErrorStamped>::SharedPtr pub_error_msg_;  // publish error message
+  rclcpp::Publisher<DrivingMonitorStamped>::SharedPtr
+    pub_driving_msg_;  // publish driving status message
 
   // Node Methods
   bool isDataReady() const;  // check if data arrive
   static bool isValidTrajectory(const Trajectory & traj);
-  boost::optional<TargetPerformanceMsgVars> computeTargetPerformanceMsgVars() const;
+  std::pair<bool, bool> computeTargetPerformanceMsgVars() const;
 
   // Callback Methods
   void onTrajectory(const Trajectory::ConstSharedPtr msg);
-  void publishErrorMsg(const TargetPerformanceMsgVars & control_performance_vars);
-  void onControlRaw(const AckermannLateralCommand::ConstSharedPtr control_msg);
+  void onControlRaw(const AckermannControlCommand::ConstSharedPtr control_msg);
   void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg);
   void onVelocity(const Odometry::ConstSharedPtr msg);
 
@@ -88,10 +100,13 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node
 
   // Parameters
   Param param_{};  // wheelbase, control period and feedback coefficients.
+  // State holder
+  std_msgs::msg::Header last_control_cmd_;
+  double d_control_cmd_{0};
 
   // Subscriber Parameters
   Trajectory::ConstSharedPtr current_trajectory_ptr_;  // ConstPtr to local traj.
-  AckermannLateralCommand::ConstSharedPtr current_control_msg_ptr_;
+  AckermannControlCommand::ConstSharedPtr current_control_msg_ptr_;
   SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_;
   Odometry::ConstSharedPtr current_odom_ptr_;
   PoseStamped::ConstSharedPtr current_pose_;  // pose of the vehicle, x, y, heading

control/control_performance_analysis/launch/controller_performance_analysis.launch.xml

@@ -1,22 +1,24 @@
 <launch>
   <arg name="control_performance_analysis_param_path" default="$(find-pkg-share control_performance_analysis)/config/control_performance_analysis.param.yaml"/>
   <arg name="input/reference_trajectory" default="/planning/scenario_planning/trajectory"/>
-  <arg name="input/control_raw" default="/control/trajectory_follower/lateral/control_cmd"/>
+  <arg name="input/control_raw" default="/control/command/control_cmd"/>
   <arg name="input/measured_steering" default="/vehicle/status/steering_status"/>
   <arg name="input/current_odometry" default="/localization/kinematic_state"/>
   <arg name="output/error_stamped" default="/control_performance/performance_vars"/>
+  <arg name="output/driving_status_stamped" default="/control_performance/driving_status"/>
 
   <!-- vehicle info -->
   <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
 
-  <node pkg="control_performance_analysis" exec="control_performance_analysis" name="control_performance_analysis" output="screen">
+  <node pkg="control_performance_analysis" exec="control_performance_analysis_exe" name="control_performance_analysis_exe" output="screen">
     <param from="$(var control_performance_analysis_param_path)"/>
     <param from="$(var vehicle_info_param_file)"/>
 
     <remap from="~/input/reference_trajectory" to="$(var input/reference_trajectory)"/>
     <remap from="~/input/control_raw" to="$(var input/control_raw)"/>
     <remap from="~/input/measured_steering" to="$(var input/measured_steering)"/>
-    <remap from="~/input/current_odometry" to="$(var input/current_odometry)"/>
+    <remap from="~/input/odometry" to="$(var input/current_odometry)"/>
     <remap from="~/output/error_stamped" to="$(var output/error_stamped)"/>
+    <remap from="~/output/driving_status_stamped" to="$(var output/driving_status_stamped)"/>
   </node>
 </launch>

control/control_performance_analysis/msg/DrivingMonitor.msg

@@ -1,6 +1,5 @@
-float64 longitudinal_acceleration
-float64 longitudinal_jerk
-float64 lateral_acceleration
-float64 lateral_jerk
-float64 tracking_curvature_discontinuity_ability
-float64 controller_processing_time
+control_performance_analysis/FloatStamped longitudinal_acceleration
+control_performance_analysis/FloatStamped longitudinal_jerk
+control_performance_analysis/FloatStamped lateral_acceleration
+control_performance_analysis/FloatStamped lateral_jerk
+control_performance_analysis/FloatStamped controller_processing_time

control/control_performance_analysis/msg/DrivingMonitorStamped.msg

@@ -1,2 +1 @@
-std_msgs/Header header
 control_performance_analysis/DrivingMonitor driving_monitor

control/control_performance_analysis/msg/Error.msg

@@ -7,4 +7,5 @@ float64 curvature_estimate
 float64 curvature_estimate_pp
 float64 lateral_error_velocity
 float64 lateral_error_acceleration
-float64 longitudinal_velocity_error
+float64 longitudinal_velocity_error
+float64 tracking_curvature_discontinuity_ability

control/control_performance_analysis/msg/FloatStamped.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+float64 data

control/control_performance_analysis/package.xml

@@ -15,22 +15,27 @@
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>control_performance_analysis_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>libboost-dev</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
+  <depend>signal_processing</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>vehicle_info_util</depend>
 
+  <build_depend>builtin_interfaces</build_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>global_parameter_loader</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>