diff --git a/control/control_performance_analysis/CMakeLists.txt b/control/control_performance_analysis/CMakeLists.txt index 0c877fb49063b..10a305e1f7dbc 100644 --- a/control/control_performance_analysis/CMakeLists.txt +++ b/control/control_performance_analysis/CMakeLists.txt @@ -4,6 +4,7 @@ project(control_performance_analysis) find_package(autoware_cmake REQUIRED) autoware_package() + find_package(Boost REQUIRED) find_package(eigen3_cmake_module REQUIRED) @@ -13,43 +14,56 @@ include_directories( SYSTEM ${EIGEN3_INCLUDE_DIR} ) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) -rosidl_generate_interfaces(control_performance_analysis_msgs - msg/Error.msg - msg/ErrorStamped.msg - DEPENDENCIES - std_msgs +rosidl_generate_interfaces( + control_performance_analysis + "msg/Error.msg" + "msg/ErrorStamped.msg" + "msg/DrivingMonitorStamped.msg" + "msg/FloatStamped.msg" + DEPENDENCIES builtin_interfaces std_msgs ) -ament_auto_add_library(control_performance_analysis_core SHARED - src/control_performance_analysis_utils.cpp - src/control_performance_analysis_core.cpp +ament_auto_add_library( + control_performance_analysis_core SHARED + src/control_performance_analysis_utils.cpp + src/control_performance_analysis_core.cpp ) -ament_auto_add_library(control_performance_analysis_node SHARED - src/control_performance_analysis_node.cpp +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") + rosidl_target_interfaces(control_performance_analysis_node + control_performance_analysis "rosidl_typesupport_cpp") + rosidl_target_interfaces(control_performance_analysis_core + control_performance_analysis "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}") + rosidl_get_typesupport_target( + cpp_typesupport_target control_performance_analysis "rosidl_typesupport_cpp") + target_link_libraries(control_performance_analysis_node "${cpp_typesupport_target}") + target_link_libraries(control_performance_analysis_core "${cpp_typesupport_target}") + endif() -target_link_libraries(control_performance_analysis_node - control_performance_analysis_core +target_link_libraries( + control_performance_analysis_node + control_performance_analysis_core ) -rclcpp_components_register_node(control_performance_analysis_node - PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode" - EXECUTABLE control_performance_analysis +rclcpp_components_register_node( + control_performance_analysis_node + PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode" + EXECUTABLE control_performance_analysis_exe ) ament_auto_package( - INSTALL_TO_SHARE - launch - config + INSTALL_TO_SHARE + launch + config ) diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index dbb0405590b50..bb698f9c9d497 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -2,27 +2,92 @@ ## Purpose -`control_performance_analysis` is the package to analyze the tracking performance of a control module. +`control_performance_analysis` is the package to analyze the tracking performance of a control module and monitor the driving status of the vehicle. This package is used as a tool to quantify the results of the control module. That's why it doesn't interfere with the core logic of autonomous driving. Based on the various input from planning, control, and vehicle, it publishes the result of analysis as `control_performance_analysis::msg::ErrorStamped` defined in this package. +All results in `ErrorStamped` message are calculated in Frenet Frame of curve. Errors and velocity errors are calculated by using paper below. + +`Werling, Moritz & Groell, Lutz & Bretthauer, Georg. (2010). Invariant Trajectory Tracking With a Full-Size Autonomous Road Vehicle. Robotics, IEEE Transactions on. 26. 758 - 765. 10.1109/TRO.2010.2052325.` + +If you are interested in calculations, you can see the error and error velocity calculations in section `C. Asymptotical Trajectory Tracking With Orientation Control`. + +Error acceleration calculations are made based on the velocity calculations above. You can see below the calculation of error acceleration. + +![CodeCogsEqn](https://user-images.githubusercontent.com/45468306/169027099-ef15b306-2868-4084-a350-0e2b652c310f.png) + ## Input / Output ### Input topics -| Name | Type | Description | -| -------------------------------------------------- | -------------------------------------------------------- | --------------------------------------------------- | -| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. | -| `/control/trajectory_follower/lateral/control_cmd` | autoware_auto_control_msgs::msg::AckermannLateralCommand | Output lateral control command from control module. | -| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | -| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | -| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | +| Name | Type | Description | +| ---------------------------------------- | -------------------------------------------------------- | ------------------------------------------- | +| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. | +| `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | Output control command from control module. | +| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | +| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | +| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | ### Output topics -| Name | Type | Description | -| --------------------------------------- | ----------------------------------------------- | --------------------------------------- | -| `/control_performance/performance_vars` | control_performance_analysis::msg::ErrorStamped | The result of the performance analysis. | +| Name | Type | Description | +| --------------------------------------- | -------------------------------------------------------- | --------------------------------------------------- | +| `/control_performance/performance_vars` | control_performance_analysis::msg::ErrorStamped | The result of the performance analysis. | +| `/control_performance/driving_status` | control_performance_analysis::msg::DrivingMonitorStamped | Driving status (acceleration, jerk etc.) monitoring | + +### Outputs + +#### control_performance_analysis::msg::DrivingMonitorStamped + +| Name | Type | Description | +| ---------------------------- | ----- | -------------------------------------------------------- | +| `longitudinal_acceleration` | float | [m / s^2] | +| `longitudinal_jerk` | float | [m / s^3] | +| `lateral_acceleration` | float | [m / s^2] | +| `lateral_jerk` | float | [m / s^3] | +| `controller_processing_time` | float | Timestamp between last two control command messages [ms] | + +#### control_performance_analysis::msg::ErrorStamped + +| Name | Type | Description | +| ------------------------------------------ | ----- | ----------------------------------------------------------------------------------------------------------------- | +| `lateral_error` | float | [m] | +| `lateral_error_velocity` | float | [m / s] | +| `lateral_error_acceleration` | float | [m / s^2] | +| `longitudinal_error` | float | [m] | +| `longitudinal_error_velocity` | float | [m / s] | +| `longitudinal_error_acceleration` | float | [m / s^2] | +| `heading_error` | float | [rad] | +| `heading_error_velocity` | float | [rad / s] | +| `control_effort_energy` | float | [u * R * u^T] | +| `error_energy` | float | lateral_error^2 + heading_error^2 | +| `value_approximation` | float | V = xPx' ; Value function from DARE Lyap matrix P | +| `curvature_estimate` | float | [1 / m] | +| `curvature_estimate_pp` | float | [1 / m] | +| `vehicle_velocity_error` | float | [m / s] | +| `tracking_curvature_discontinuity_ability` | float | Measures the ability to tracking the curvature changes [`abs(delta(curvature)) / (1 + abs(delta(lateral_error))`] | + +## 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 | + +## Usage + +- After launched simulation and control module, launch the `control_performance_analysis.launch.xml`. +- You should be able to see the driving monitor and error variables in topics. +- If you want to visualize the results, you can use `Plotjuggler` and use `config/controller_monitor.xml` as layout. +- After import the layout, please specify the topics that are listed below. + +> - /localization/kinematic_state +> - /control_performance/driving_status +> - /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. diff --git a/control/control_performance_analysis/config/control_performance_analysis.param.yaml b/control/control_performance_analysis/config/control_performance_analysis.param.yaml index 9867e170e9adb..cca4a02653690 100644 --- a/control/control_performance_analysis/config/control_performance_analysis.param.yaml +++ b/control/control_performance_analysis/config/control_performance_analysis.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: # -- publishing period -- - control_period: 0.033 - double curvature_interval_length_: 5.0 + curvature_interval_length_: 5.0 + prevent_zero_division_value: 0.001 + odom_interval_: 3 + acceptable_min_waypoint_distance: 2.0 diff --git a/control/control_performance_analysis/config/controller_monitor.xml b/control/control_performance_analysis/config/controller_monitor.xml new file mode 100644 index 0000000000000..c4247c3a9d0a3 --- /dev/null +++ b/control/control_performance_analysis/config/controller_monitor.xml @@ -0,0 +1,534 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/longitudinal_error_acceleration + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/longitudinal_error_velocity + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/longitudinal_error + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/lateral_error_acceleration + + + sum = 0 + sum = sum + math.sqrt(value*value) + +return sum + /control_performance/performance_vars/error/heading_error + + + sum = 0 + sum = sum + math.sqrt(value*value) + +return sum + /control_performance/performance_vars/error/heading_error_velocity + + + sum = 0 + sum = sum + math.sqrt(value*value) + +return sum + /control_performance/performance_vars/error/lateral_error + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/longitudinal_error_velocity + + + sum = 0 + sum = sum + math.sqrt(value*value) + +return sum + /control_performance/performance_vars/error/lateral_error_velocity + + + sum = 0 + sum = sum + math.sqrt(value*value) + +return sum + /control_performance/performance_vars/error/lateral_error_acceleration + + + sum = 0 + sum = sum + math.sqrt(value*value) + +return sum + /control_performance/performance_vars/error/longitudinal_error + + + sum = 0 + sum = sum + math.sqrt(value*value) + +return sum + /control_performance/performance_vars/error/longitudinal_error_acceleration + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/heading_error + + + sum = 0 + sum = sum + math.sqrt(value*value) + +return sum + /control_performance/performance_vars/error/tracking_curvature_discontinuity_ability + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/tracking_curvature_discontinuity_ability + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/lateral_error + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/heading_error_velocity + + + sum = 0 + sum = sum + math.abs(value) + +return sum + /control_performance/performance_vars/error/lateral_error_velocity + + + + + diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 4ffcc63f81473..711301271cb53 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,14 +16,20 @@ #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 +#include -#include +#include #include +#include #include #include #include +#include #include #include @@ -31,24 +37,16 @@ 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 { @@ -56,14 +54,18 @@ class ControlPerformanceAnalysisCore // 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, + double acceptable_min_waypoint_distance, double prevent_zero_division_value); // 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, double & interpolated_acceleration); + void setOdomHistory(const Odometry & odom); + void setSteeringStatus(const SteeringReport & steering); void findCurveRefIdx(); std::pair findClosestPrevWayPointIdx_path_direction(); @@ -72,26 +74,46 @@ class ControlPerformanceAnalysisCore // Getters bool isDataReady() const; - std::pair getPerformanceVars(); - Pose getPrevWPPose() const; + bool calculateErrorVars(); + bool calculateDrivingVars(); + Pose getPrevWPPose() const; // It is not used! std::pair calculateClosestPose(); + // Output variables + ErrorStamped error_vars; + DrivingMonitorStamped driving_status_vars; + private: double wheelbase_; double curvature_interval_length_; + uint odom_interval_; + double acceptable_min_waypoint_distance_; + double prevent_zero_division_value_; // Variables Received Outside std::shared_ptr current_waypoints_ptr_; + std::shared_ptr> current_waypoints_vel_ptr_; std::shared_ptr current_vec_pose_ptr_; - std::shared_ptr> current_velocities_ptr_; // [Vx, Heading rate] - std::shared_ptr current_control_ptr_; + std::shared_ptr> odom_history_ptr_; // velocities at k-2, k-1, k, k+1 + std::shared_ptr current_control_ptr_; + std::shared_ptr current_vec_steering_msg_ptr_; + + // State holder + + std_msgs::msg::Header last_odom_header; + std_msgs::msg::Header last_steering_report; // Variables computed + std::unique_ptr idx_prev_wp_; // the waypoint index, vehicle std::unique_ptr idx_curve_ref_wp_; // index of waypoint corresponds to front axle center std::unique_ptr idx_next_wp_; // the next waypoint index, vehicle heading to - std::unique_ptr prev_target_vars_{}; + std::unique_ptr prev_target_vars_{}; + std::unique_ptr prev_driving_vars_{}; std::shared_ptr interpolated_pose_ptr_; + std::shared_ptr interpolated_velocity_ptr_; + std::shared_ptr interpolated_acceleration_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 diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 9f051534d2972..a47ebc45b3a74 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -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 +#include #include -#include +#include #include #include #include @@ -30,12 +32,14 @@ #include #include +#include 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; @@ -46,9 +50,12 @@ struct Param // Global parameters double wheel_base; double curvature_interval_length; + double prevent_zero_division_value; + uint odom_interval; // Increase it for smoother curve - // Control Method Parameters - double control_period; + // How far the next waypoint can be ahead of the vehicle direction. + + double acceptable_min_waypoint_distance; }; class ControlPerformanceAnalysisNode : public rclcpp::Node @@ -59,8 +66,8 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node private: // Subscribers and Local Variable Assignment rclcpp::Subscription::SharedPtr sub_trajectory_; // subscribe to trajectory - rclcpp::Subscription::SharedPtr - sub_control_steering_; // subscribe to steering control value + rclcpp::Subscription::SharedPtr + sub_control_cmd_; // subscribe to steering control value rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity rclcpp::Subscription::SharedPtr sub_vehicle_steering_; @@ -69,33 +76,39 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Publishers rclcpp::Publisher::SharedPtr pub_error_msg_; // publish error message + rclcpp::Publisher::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 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); - // Timer - To Publish In Control Period - rclcpp::TimerBase::SharedPtr timer_publish_; - void onTimer(); - // 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 + // prev states + Trajectory prev_traj; + AckermannControlCommand prev_cmd; + SteeringReport prev_steering; + + std_msgs::msg::Header odom_state_; + // Algorithm std::unique_ptr control_performance_core_ptr_; }; diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp index de05ea3da63f3..7f982bd4aa9e1 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -41,6 +41,23 @@ inline std::vector getNormalVector(double yaw_angle) return std::vector{-sin(yaw_angle), cos(yaw_angle)}; } +inline std::vector computeLateralLongitudinalError( + std::vector & closest_point_position, std::vector & vehicle_position, + double & desired_yaw_angle) +{ + // Vector to path point originating from the vehicle r - rd + std::vector vector_to_path_point{ + vehicle_position[0] - closest_point_position[0], + vehicle_position[1] - closest_point_position[1]}; + + double lateral_error = -sin(desired_yaw_angle) * vector_to_path_point[0] + + cos(desired_yaw_angle) * vector_to_path_point[1]; + double longitudinal_error = cos(desired_yaw_angle) * vector_to_path_point[0] + + sin(desired_yaw_angle) * vector_to_path_point[1]; + + return std::vector{lateral_error, longitudinal_error}; +} + inline double computeLateralError( std::vector & closest_point_position, std::vector & vehicle_position, double & yaw_angle) diff --git a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml index dfec99db7d7c0..be2b30bdf1bf9 100644 --- a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml +++ b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml @@ -1,22 +1,24 @@ - + + - + - + + diff --git a/control/control_performance_analysis/msg/DrivingMonitorStamped.msg b/control/control_performance_analysis/msg/DrivingMonitorStamped.msg new file mode 100644 index 0000000000000..c53592bc889ed --- /dev/null +++ b/control/control_performance_analysis/msg/DrivingMonitorStamped.msg @@ -0,0 +1,5 @@ +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 diff --git a/control/control_performance_analysis/msg/Error.msg b/control/control_performance_analysis/msg/Error.msg index f63ed2117ddce..f689034f3d582 100644 --- a/control/control_performance_analysis/msg/Error.msg +++ b/control/control_performance_analysis/msg/Error.msg @@ -1,9 +1,15 @@ float64 lateral_error +float64 lateral_error_velocity +float64 lateral_error_acceleration +float64 longitudinal_error +float64 longitudinal_error_velocity +float64 longitudinal_error_acceleration float64 heading_error +float64 heading_error_velocity float64 control_effort_energy float64 error_energy float64 value_approximation float64 curvature_estimate float64 curvature_estimate_pp -float64 lateral_error_velocity -float64 lateral_error_acceleration +float64 vehicle_velocity_error +float64 tracking_curvature_discontinuity_ability diff --git a/control/control_performance_analysis/msg/FloatStamped.msg b/control/control_performance_analysis/msg/FloatStamped.msg new file mode 100644 index 0000000000000..5a609c9301a28 --- /dev/null +++ b/control/control_performance_analysis/msg/FloatStamped.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +float64 data diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 4b4d2f8e61c24..7c60e0a784749 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -5,12 +5,15 @@ 0.1.0 Controller Performance Evaluation Ali Boyali + Berkay Karaman + Apache License 2.0 ament_cmake_auto rosidl_default_generators autoware_cmake + builtin_interfaces autoware_auto_control_msgs autoware_auto_planning_msgs @@ -21,16 +24,16 @@ rclcpp rclcpp_components sensor_msgs + signal_processing std_msgs tf2 tf2_eigen tf2_ros tier4_autoware_utils vehicle_info_util - + builtin_interfaces global_parameter_loader rosidl_default_runtime - ament_lint_auto autoware_lint_common diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 3a5f593818946..79e0ccd86dd26 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -26,25 +26,52 @@ using geometry_msgs::msg::Quaternion; ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() : wheelbase_{2.74} { - prev_target_vars_ = std::make_unique(TargetPerformanceMsgVars{}); - current_velocities_ptr_ = std::make_shared>(2, 0.0); + prev_target_vars_ = std::make_unique(); + prev_driving_vars_ = std::make_unique(); + odom_history_ptr_ = std::make_shared>(); + odom_interval_ = 0; + curvature_interval_length_ = 10.0; + acceptable_min_waypoint_distance_ = 2.0; + prevent_zero_division_value_ = 0.001; } ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore( - double wheelbase, double curvature_interval_length) -: wheelbase_{wheelbase}, curvature_interval_length_{curvature_interval_length} + double wheelbase, double curvature_interval_length, uint odom_interval, + double acceptable_min_waypoint_distance, double prevent_zero_division_value) +: wheelbase_{wheelbase}, + curvature_interval_length_{curvature_interval_length}, + odom_interval_{odom_interval}, + acceptable_min_waypoint_distance_{acceptable_min_waypoint_distance}, + prevent_zero_division_value_{prevent_zero_division_value} { // prepare control performance struct - prev_target_vars_ = std::make_unique(TargetPerformanceMsgVars{}); - current_velocities_ptr_ = std::make_shared>(2, 0.0); + prev_target_vars_ = std::make_unique(); + odom_history_ptr_ = std::make_shared>(); } void ControlPerformanceAnalysisCore::setCurrentWaypoints(const Trajectory & trajectory) { current_waypoints_ptr_ = std::make_shared(); + current_waypoints_vel_ptr_ = std::make_shared>(); for (const auto & point : trajectory.points) { current_waypoints_ptr_->poses.emplace_back(point.pose); + current_waypoints_vel_ptr_->emplace_back(point.longitudinal_velocity_mps); + } +} + +void ControlPerformanceAnalysisCore::setOdomHistory(const Odometry & odom) +{ + // We need to take the odometry history to calculate jerk and acceleration + if (!odom_history_ptr_->empty() && odom.header.stamp == odom_history_ptr_->back().header.stamp) { + return; + } + if (odom_history_ptr_->size() >= (3 + odom_interval_ * 2)) { + // If higher, remove the first element of vector + odom_history_ptr_->erase(odom_history_ptr_->begin()); + odom_history_ptr_->push_back(odom); + } else { + odom_history_ptr_->push_back(odom); } } @@ -53,9 +80,9 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) current_vec_pose_ptr_ = std::make_shared(msg); } -void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannLateralCommand & msg) +void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannControlCommand & msg) { - current_control_ptr_ = std::make_shared(msg); + current_control_ptr_ = std::make_shared(msg); } std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPointIdx_path_direction() @@ -64,16 +91,13 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - // How far the next waypoint can be ahead of the vehicle direction. - double acceptable_min_distance = 2.0; - /* * Create Vectors of Path Directions for each interval * interval_vector_xy = {waypoint_1 - waypoint_0}_xy * */ // Prepare vector of projection distance values; projection of vehicle vectors onto the intervals - std::vector projection_distances_ds; // + std::vector projection_distances_ds; auto f_projection_dist = [this](auto pose_1, auto pose_0) { // Vector of intervals. @@ -88,8 +112,7 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint this->current_vec_pose_ptr_->position.x - pose_0.position.x, this->current_vec_pose_ptr_->position.y - pose_0.position.y}; - double projection_distance_onto_interval; - projection_distance_onto_interval = + double projection_distance_onto_interval = (int_vec[0] * vehicle_vec[0] + int_vec[1] * vehicle_vec[1]) / ds_mag; return projection_distance_onto_interval; @@ -124,10 +147,7 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint int32_t length_of_trajectory = std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); - // Find and set the waypoint L-wheelbase meters ahead of the current waypoint. - findCurveRefIdx(); - - return ((min_distance_ds <= acceptable_min_distance) & (*idx_prev_wp_ >= 0) & + return ((min_distance_ds <= acceptable_min_waypoint_distance_) && (*idx_prev_wp_ >= 0) && (*idx_prev_wp_ <= length_of_trajectory)) ? std::make_pair(true, *idx_prev_wp_) : std::make_pair(false, std::numeric_limits::quiet_NaN()); @@ -142,17 +162,22 @@ bool ControlPerformanceAnalysisCore::isDataReady() const return false; } - if (!current_waypoints_ptr_) { + if (current_waypoints_ptr_->poses.empty()) { RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "cannot get current trajectory waypoints ..."); return false; } - if (!current_velocities_ptr_) { - RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for current_velocity ..."); + if (odom_history_ptr_->size() < 2) { + RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for odometry data ..."); return false; } if (!current_control_ptr_) { + RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for current_control_cmd ..."); + return false; + } + + if (!current_vec_steering_msg_ptr_) { RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for current_steering ..."); return false; } @@ -160,23 +185,27 @@ bool ControlPerformanceAnalysisCore::isDataReady() const return true; } -std::pair ControlPerformanceAnalysisCore::getPerformanceVars() +bool ControlPerformanceAnalysisCore::calculateErrorVars() { // Check if data is ready. if (!isDataReady() || !idx_prev_wp_) { - return std::make_pair(false, TargetPerformanceMsgVars{}); + return false; } - TargetPerformanceMsgVars target_vars{}; - // Get the interpolated pose std::pair pair_pose_interp_wp_ = calculateClosestPose(); - if (!pair_pose_interp_wp_.first || !interpolated_pose_ptr_) { - RCLCPP_WARN_THROTTLE( - logger_, clock_, 1000, "Cannot get interpolated pose into control_performance algorithm"); + // Find and set the waypoint L-wheelbase meters ahead of the current waypoint. + findCurveRefIdx(); - return std::make_pair(false, TargetPerformanceMsgVars{}); + if ( + !pair_pose_interp_wp_.first || !interpolated_pose_ptr_ || !interpolated_velocity_ptr_ || + !interpolated_acceleration_ptr_) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, + "Cannot get interpolated pose, velocity, and acceleration into control_performance " + "algorithm"); + return false; } auto && pose_interp_wp_ = pair_pose_interp_wp_.second; @@ -189,50 +218,182 @@ std::pair ControlPerformanceAnalysisCore::getPer current_vec_pose_ptr_->position.x, current_vec_pose_ptr_->position.y}; // Get Yaw angles of the reference waypoint and the vehicle - double const target_yaw = tf2::getYaw(pose_interp_wp_.orientation); + double target_yaw = tf2::getYaw(pose_interp_wp_.orientation); double vehicle_yaw_angle = tf2::getYaw(current_vec_pose_ptr_->orientation); // Compute Curvature at the point where the front axle might follow // get the waypoint corresponds to the front_axle center if (!idx_curve_ref_wp_) { - RCLCPP_ERROR(logger_, "Cannot find index of curvature reference waypoint "); - return std::make_pair(false, TargetPerformanceMsgVars{}); + RCLCPP_WARN(logger_, "Cannot find index of curvature reference waypoint "); + return false; } double curvature_est = estimateCurvature(); // three point curvature double curvature_est_pp = estimatePurePursuitCurvature(); // pure pursuit curvature - // Compute lateral error - projection of vector to waypoint from vehicle onto the vehicle normal. - double && lateral_error = - utils::computeLateralError(interp_waypoint_xy, vehicle_position_xy, vehicle_yaw_angle); + // Compute lateral, longitudinal, heading error w.r.t. frenet frame + + std::vector lateral_longitudinal_error = + utils::computeLateralLongitudinalError(interp_waypoint_xy, vehicle_position_xy, target_yaw); + double & lateral_error = lateral_longitudinal_error[0]; + double & longitudinal_error = lateral_longitudinal_error[1]; // Compute the yaw angle error. double && heading_yaw_error = utils::angleDistance(vehicle_yaw_angle, target_yaw); - // Set the values of TargetPerformanceMsgVars. - target_vars.lateral_error = lateral_error; - target_vars.heading_error = heading_yaw_error; - - double steering_val = current_control_ptr_->steering_tire_angle; - target_vars.control_effort_energy = contR * steering_val * steering_val; // u*R*u'; + // Set the values of ErrorMsgVars. + + error_vars.error.lateral_error = lateral_error; + error_vars.error.longitudinal_error = longitudinal_error; + error_vars.error.heading_error = heading_yaw_error; + + // odom history contains k + 1, k, k - 1 ... steps. We are in kth step + uint && odom_size = odom_history_ptr_->size(); + + error_vars.header.stamp = odom_history_ptr_->at(odom_size - 2).header.stamp; // we are in step k + + double & Vx = odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; + // Current acceleration calculation + double && d_x = odom_history_ptr_->at(odom_size - 1).pose.pose.position.x - + odom_history_ptr_->at(odom_size - 2).pose.pose.position.x; + double && d_y = odom_history_ptr_->at(odom_size - 1).pose.pose.position.y - + odom_history_ptr_->at(odom_size - 2).pose.pose.position.y; + double && ds = std::hypot(d_x, d_y); + + double && vel_mean = (odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x + + odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x) / + 2.0; + double && dv = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x - + odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x; + double && dt = ds / std::max(vel_mean, prevent_zero_division_value_); + double && Ax = dv / std::max(dt, prevent_zero_division_value_); // current acceleration + + double && longitudinal_error_velocity = + Vx * cos(heading_yaw_error) - *interpolated_velocity_ptr_ * (1 - curvature_est * lateral_error); + double && lateral_error_velocity = + Vx * sin(heading_yaw_error) - *interpolated_velocity_ptr_ * curvature_est * longitudinal_error; + + double && steering_cmd = current_control_ptr_->lateral.steering_tire_angle; + double && current_steering_val = current_vec_steering_msg_ptr_->steering_tire_angle; + error_vars.error.control_effort_energy = contR * steering_cmd * steering_cmd; // u*R*u'; + + double && heading_velocity_error = (Vx * tan(current_steering_val)) / wheelbase_ - + *this->interpolated_velocity_ptr_ * curvature_est; + + double && lateral_acceleration_error = + -curvature_est * *interpolated_acceleration_ptr_ * longitudinal_error - + curvature_est * *interpolated_velocity_ptr_ * longitudinal_error_velocity + + Vx * heading_velocity_error * cos(heading_yaw_error) + Ax * sin(heading_yaw_error); + + double && longitudinal_acceleration_error = + curvature_est * *interpolated_acceleration_ptr_ * lateral_error + + curvature_est * *interpolated_velocity_ptr_ * lateral_error_velocity - + Vx * heading_velocity_error * sin(heading_yaw_error) + Ax * cos(heading_yaw_error) - + *interpolated_acceleration_ptr_; + + error_vars.error.lateral_error_velocity = lateral_error_velocity; + error_vars.error.lateral_error_acceleration = lateral_acceleration_error; + error_vars.error.longitudinal_error_velocity = longitudinal_error_velocity; + error_vars.error.longitudinal_error_acceleration = longitudinal_acceleration_error; + error_vars.error.heading_error_velocity = heading_velocity_error; Eigen::Vector2d error_vec; error_vec << lateral_error, heading_yaw_error; - target_vars.error_energy = error_vec.dot(error_vec); - target_vars.value_approximation = error_vec.transpose() * lyap_P_ * error_vec; // x'Px + error_vars.error.error_energy = error_vec.dot(error_vec); + error_vars.error.value_approximation = error_vec.transpose() * lyap_P_ * error_vec; // x'Px - target_vars.curvature_estimate = curvature_est; - target_vars.curvature_estimate_pp = curvature_est_pp; + error_vars.error.curvature_estimate = curvature_est; + error_vars.error.curvature_estimate_pp = curvature_est_pp; - double Vx = current_velocities_ptr_->at(0); - target_vars.lateral_error_velocity = Vx * sin(heading_yaw_error); - target_vars.lateral_error_acceleration = Vx * tan(steering_val) / wheelbase_ - curvature_est * Vx; + error_vars.error.vehicle_velocity_error = Vx - *this->interpolated_velocity_ptr_; + error_vars.error.tracking_curvature_discontinuity_ability = + (std::fabs(curvature_est - prev_target_vars_->error.curvature_estimate)) / + (1 + std::fabs(lateral_error - prev_target_vars_->error.lateral_error)); - prev_target_vars_ = std::move(std::make_unique(target_vars)); + prev_target_vars_ = std::make_unique(error_vars); + + return true; +} - return std::make_pair(true, target_vars); +bool ControlPerformanceAnalysisCore::calculateDrivingVars() +{ + if (!odom_history_ptr_->empty()) { + const uint odom_size = odom_history_ptr_->size(); + + if (odom_history_ptr_->at(odom_size - 1).header.stamp != last_odom_header.stamp) { + // Calculate lateral acceleration + + driving_status_vars.lateral_acceleration.header.set__stamp( + odom_history_ptr_->at(odom_size - 1).header.stamp); + driving_status_vars.lateral_acceleration.data = + odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x * + tan(current_vec_steering_msg_ptr_->steering_tire_angle) / wheelbase_; + + if (odom_history_ptr_->size() >= odom_interval_ + 2) { + // Calculate longitudinal acceleration + + double dv = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x - + odom_history_ptr_->at(odom_size - odom_interval_ - 2).twist.twist.linear.x; + + auto duration = + (rclcpp::Time(odom_history_ptr_->at(odom_size - 1).header.stamp) - + rclcpp::Time(odom_history_ptr_->at(odom_size - odom_interval_ - 2).header.stamp)); + + double dt = duration.seconds(); + + driving_status_vars.longitudinal_acceleration.data = dv / dt; + driving_status_vars.longitudinal_acceleration.header.set__stamp( + rclcpp::Time(odom_history_ptr_->at(odom_size - odom_interval_ - 2).header.stamp) + + duration * 0.5); // Time stamp of acceleration data + + // Calculate lateral jerk + + double d_lateral_jerk = driving_status_vars.lateral_acceleration.data - + prev_driving_vars_->lateral_acceleration.data; + + // We already know the delta time from above. same as longitudinal acceleration + + driving_status_vars.lateral_jerk.data = d_lateral_jerk / dt; + driving_status_vars.lateral_jerk.header = + driving_status_vars.longitudinal_acceleration.header; + } + + if (odom_history_ptr_->size() == 2 * odom_interval_ + 3) { + // calculate longitudinal jerk + double d_a = driving_status_vars.longitudinal_acceleration.data - + prev_driving_vars_->longitudinal_acceleration.data; + auto duration = + (rclcpp::Time(driving_status_vars.longitudinal_acceleration.header.stamp) - + rclcpp::Time(prev_driving_vars_->longitudinal_acceleration.header.stamp)); + double dt = duration.seconds(); + driving_status_vars.longitudinal_jerk.data = d_a / dt; + driving_status_vars.longitudinal_jerk.header.set__stamp( + rclcpp::Time(prev_driving_vars_->longitudinal_acceleration.header.stamp) + + duration * 0.5); // Time stamp of jerk data + } + + prev_driving_vars_ = + std::move(std::make_unique(driving_status_vars)); + + last_odom_header.stamp = odom_history_ptr_->at(odom_size - 1).header.stamp; + last_steering_report.stamp = current_vec_steering_msg_ptr_->stamp; + + } else if (last_steering_report.stamp != current_vec_steering_msg_ptr_->stamp) { + driving_status_vars.lateral_acceleration.header.set__stamp( + current_vec_steering_msg_ptr_->stamp); + driving_status_vars.lateral_acceleration.data = + odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x * + tan(current_vec_steering_msg_ptr_->steering_tire_angle) / wheelbase_; + last_steering_report.stamp = current_vec_steering_msg_ptr_->stamp; + } + return true; + + } else { + RCLCPP_ERROR(logger_, "Can not get odometry data! "); + return false; + } } Pose ControlPerformanceAnalysisCore::getPrevWPPose() const @@ -240,11 +401,12 @@ Pose ControlPerformanceAnalysisCore::getPrevWPPose() const Pose pose_ref_waypoint_ = current_waypoints_ptr_->poses.at(*idx_prev_wp_); return pose_ref_waypoint_; } -void ControlPerformanceAnalysisCore::setCurrentVelocities(const Twist & twist_msg) + +void ControlPerformanceAnalysisCore::setSteeringStatus(const SteeringReport & steering) { - current_velocities_ptr_->at(0) = twist_msg.linear.x; - current_velocities_ptr_->at(1) = twist_msg.angular.z; + current_vec_steering_msg_ptr_ = std::make_shared(steering); } + void ControlPerformanceAnalysisCore::findCurveRefIdx() { // Get the previous waypoint as the reference @@ -287,6 +449,9 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() // Define the next waypoint - so that we can define an interval in which the car follow a line. int32_t idx_next_wp_temp; + double next_wp_acc = 0.0; + double prev_wp_acc = 0.0; + int32_t total_num_of_waypoints_in_traj = std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); @@ -306,17 +471,21 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() * */ // First get te yaw angles of all three poses. - double prev_yaw = tf2::getYaw(current_waypoints_ptr_->poses.at(*idx_prev_wp_).orientation); - double next_yaw = tf2::getYaw(current_waypoints_ptr_->poses.at(*idx_next_wp_).orientation); + double && prev_yaw = tf2::getYaw(current_waypoints_ptr_->poses.at(*idx_prev_wp_).orientation); + double && next_yaw = tf2::getYaw(current_waypoints_ptr_->poses.at(*idx_next_wp_).orientation); + + double & prev_velocity = current_waypoints_vel_ptr_->at(*idx_prev_wp_); + double & next_velocity = current_waypoints_vel_ptr_->at(*idx_next_wp_); // Previous waypoint to next waypoint. - double dx_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.x - - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; + double && dx_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.x - + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; - double dy_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y - - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; + double && dy_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y - + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; - double delta_psi_prev2next = utils::angleDistance(next_yaw, prev_yaw); + double && delta_psi_prev2next = utils::angleDistance(next_yaw, prev_yaw); + double && d_vel_prev2next = next_velocity - prev_velocity; // Create a vector from p0 (prev) --> p1 (to next wp) std::vector v_prev2next_wp{dx_prev2next, dy_prev2next}; @@ -328,23 +497,23 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() * * */ - double dx_prev2vehicle = + double && dx_prev2vehicle = current_vec_pose_ptr_->position.x - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; - double dy_prev2vehicle = + double && dy_prev2vehicle = current_vec_pose_ptr_->position.y - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; // Vector from p0 to p_vehicle std::vector v_prev2vehicle{dx_prev2vehicle, dy_prev2vehicle}; // Compute the length of v_prev2next_wp : vector from p0 --> p1 - double distance_p02p1 = std::hypot(dx_prev2next, dy_prev2next); + double && distance_p02p1 = std::hypot(dx_prev2next, dy_prev2next); // Compute how far the car is away from p0 in p1 direction. p_interp is the location of the // interpolated waypoint. This is the dot product normalized by the length of the interval. // a.b = |a|.|b|.cos(alpha) -- > |a|.cos(alpha) = a.b / |b| where b is the path interval, - double distance_p02p_interp = + double && distance_p02p_interp = (dx_prev2next * dx_prev2vehicle + dy_prev2next * dy_prev2vehicle) / distance_p02p1; /* @@ -352,7 +521,7 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() * pi = p0 + ratio_t * (p1 - p0) * */ - double ratio_t = distance_p02p_interp / distance_p02p1; + double && ratio_t = distance_p02p_interp / distance_p02p1; // Interpolate pose.position and pose.orientation interpolated_pose.position.x = @@ -364,20 +533,62 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() interpolated_pose.position.z = 0.0; // Interpolate the yaw angle of pi : interpolated waypoint - double interp_yaw_angle = prev_yaw + ratio_t * delta_psi_prev2next; + double && interp_yaw_angle = prev_yaw + ratio_t * delta_psi_prev2next; + double && interp_velocity = prev_velocity + ratio_t * d_vel_prev2next; - Quaternion orient_msg = utils::createOrientationMsgFromYaw(interp_yaw_angle); + Quaternion && orient_msg = utils::createOrientationMsgFromYaw(interp_yaw_angle); interpolated_pose.orientation = orient_msg; - setInterpolatedPose(interpolated_pose); + /* interpolated acceleration calculation */ + + if (*idx_prev_wp_ == 0 || *idx_prev_wp_ == total_num_of_waypoints_in_traj - 1) { + prev_wp_acc = 0.0; + } else { + double && d_x = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.x - + current_waypoints_ptr_->poses.at(*idx_prev_wp_ - 1).position.x; + double && d_y = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y - + current_waypoints_ptr_->poses.at(*idx_prev_wp_ - 1).position.y; + double && ds = std::hypot(d_x, d_y); + double && vel_mean = (current_waypoints_vel_ptr_->at(*idx_next_wp_) + + current_waypoints_vel_ptr_->at(*idx_prev_wp_ - 1)) / + 2.0; + double && dv = current_waypoints_vel_ptr_->at(*idx_next_wp_) - + current_waypoints_vel_ptr_->at(*idx_prev_wp_ - 1); + double && dt = ds / std::max(vel_mean, prevent_zero_division_value_); + prev_wp_acc = dv / std::max(dt, prevent_zero_division_value_); + } + + if (*idx_next_wp_ == total_num_of_waypoints_in_traj - 1) { + next_wp_acc = 0.0; + } else { + double && d_x = current_waypoints_ptr_->poses.at(*idx_next_wp_ + 1).position.x - + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; + double && d_y = current_waypoints_ptr_->poses.at(*idx_next_wp_ + 1).position.y - + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; + double && ds = std::hypot(d_x, d_y); + double && vel_mean = (current_waypoints_vel_ptr_->at(*idx_next_wp_ + 1) + + current_waypoints_vel_ptr_->at(*idx_prev_wp_)) / + 2.0; + double && dv = current_waypoints_vel_ptr_->at(*idx_next_wp_ + 1) - + current_waypoints_vel_ptr_->at(*idx_prev_wp_); + double && dt = ds / std::max(vel_mean, prevent_zero_division_value_); + next_wp_acc = dv / std::max(dt, prevent_zero_division_value_); + } + double && d_acc_prev2next = next_wp_acc - prev_wp_acc; + double && interp_acceleration = prev_wp_acc + ratio_t * d_acc_prev2next; + + setInterpolatedVars(interpolated_pose, interp_velocity, interp_acceleration); return std::make_pair(true, interpolated_pose); } // Sets interpolated waypoint_ptr_. -void ControlPerformanceAnalysisCore::setInterpolatedPose(Pose & interpolated_pose) +void ControlPerformanceAnalysisCore::setInterpolatedVars( + Pose & interpolated_pose, double & interpolated_velocity, double & interpolated_acceleration) { interpolated_pose_ptr_ = std::make_shared(interpolated_pose); + interpolated_velocity_ptr_ = std::make_shared(interpolated_velocity); + interpolated_acceleration_ptr_ = std::make_shared(interpolated_acceleration); } double ControlPerformanceAnalysisCore::estimateCurvature() @@ -442,7 +653,8 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() return 0; } - double Vx = current_velocities_ptr_->at(0); + const uint32_t & odom_size = odom_history_ptr_->size(); + double & Vx = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x; double look_ahead_distance_pp = std::max(wheelbase_, 2 * Vx); auto fun_distance_cond = [this, &look_ahead_distance_pp](auto pose_t) { diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 7914f9ee24e10..58c56fea90a47 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,6 +14,7 @@ #include "control_performance_analysis/control_performance_analysis_node.hpp" +#include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" #include @@ -23,25 +24,10 @@ namespace { -using control_performance_analysis::TargetPerformanceMsgVars; +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; -ErrorStamped createPerformanceMsgVars(const TargetPerformanceMsgVars & target_performance_vars) -{ - ErrorStamped error_msgs{}; - - error_msgs.error.lateral_error = target_performance_vars.lateral_error; - error_msgs.error.heading_error = target_performance_vars.heading_error; - error_msgs.error.control_effort_energy = target_performance_vars.control_effort_energy; - error_msgs.error.error_energy = target_performance_vars.error_energy; - error_msgs.error.value_approximation = target_performance_vars.value_approximation; - error_msgs.error.curvature_estimate = target_performance_vars.curvature_estimate; - error_msgs.error.curvature_estimate_pp = target_performance_vars.curvature_estimate_pp; - error_msgs.error.lateral_error_velocity = target_performance_vars.lateral_error_velocity; - error_msgs.error.lateral_error_acceleration = target_performance_vars.lateral_error_acceleration; - - return error_msgs; -} } // namespace namespace control_performance_analysis @@ -59,19 +45,23 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( param_.wheel_base = vehicle_info.wheel_base_m; // Node Parameters. - param_.control_period = declare_parameter("control_period", 0.033); param_.curvature_interval_length = declare_parameter("curvature_interval_length", 10.0); + param_.prevent_zero_division_value = declare_parameter("prevent_zero_division_value", 0.001); + param_.odom_interval = declare_parameter("odom_interval", 2); + param_.acceptable_min_waypoint_distance = + declare_parameter("acceptable_min_waypoint_distance", 2.0); // Prepare error computation class with the wheelbase parameter. control_performance_core_ptr_ = std::make_unique( - param_.wheel_base, param_.curvature_interval_length); + param_.wheel_base, param_.curvature_interval_length, param_.odom_interval, + param_.acceptable_min_waypoint_distance, param_.prevent_zero_division_value); // Subscribers. sub_trajectory_ = create_subscription( "~/input/reference_trajectory", 1, std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1)); - sub_control_steering_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); sub_vehicle_steering_ = create_subscription( @@ -84,13 +74,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( // Publishers pub_error_msg_ = create_publisher("~/output/error_stamped", 1); - // Timer - { - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(param_.control_period)); - timer_publish_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&ControlPerformanceAnalysisNode::onTimer, this)); - } + pub_driving_msg_ = create_publisher("~/output/driving_status_stamped", 1); // Wait for first self pose self_pose_listener_.waitForFirstPose(); @@ -112,13 +96,25 @@ void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedP } void ControlPerformanceAnalysisNode::onControlRaw( - const AckermannLateralCommand::ConstSharedPtr control_msg) + const AckermannControlCommand::ConstSharedPtr control_msg) { + static bool initialized = false; if (!control_msg) { - RCLCPP_ERROR(get_logger(), "steering signal has not been received yet ..."); + RCLCPP_ERROR(get_logger(), "control command has not been received yet ..."); + return; + } else if (!initialized) { + initialized = true; + current_control_msg_ptr_ = control_msg; + last_control_cmd_.stamp = current_control_msg_ptr_->stamp; + + } else { + current_control_msg_ptr_ = control_msg; + rclcpp::Duration duration = + (rclcpp::Time(current_control_msg_ptr_->stamp) - rclcpp::Time(last_control_cmd_.stamp)); + d_control_cmd_ = duration.seconds() * 1000; // ms + last_control_cmd_.stamp = current_control_msg_ptr_->stamp; } - current_control_msg_ptr_ = control_msg; } void ControlPerformanceAnalysisNode::onVecSteeringMeasured( @@ -134,60 +130,80 @@ void ControlPerformanceAnalysisNode::onVecSteeringMeasured( void ControlPerformanceAnalysisNode::onVelocity(const Odometry::ConstSharedPtr msg) { - current_odom_ptr_ = msg; -} + // Sent previous state to error calculation because we need to calculate current acceleration. -void ControlPerformanceAnalysisNode::onTimer() -{ - // Read and Update Current Pose updating var:current_pose_. - current_pose_ = self_pose_listener_.getCurrentPose(); - - // Check Data Stream - if (!isDataReady()) { - // Publish Here + if (!current_odom_ptr_) { + if (isDataReady()) { + current_odom_ptr_ = msg; + current_pose_ = self_pose_listener_.getCurrentPose(); + control_performance_core_ptr_->setOdomHistory(*current_odom_ptr_); + prev_traj = *current_trajectory_ptr_; + prev_cmd = *current_control_msg_ptr_; + prev_steering = *current_vec_steering_msg_ptr_; + } return; } - // Compute Control Performance Variables. - auto performanceVars = computeTargetPerformanceMsgVars(); - if (!performanceVars) { - RCLCPP_ERROR(get_logger(), "steering signal has not been received yet ..."); + control_performance_core_ptr_->setCurrentWaypoints(prev_traj); + control_performance_core_ptr_->setCurrentPose(current_pose_->pose); + control_performance_core_ptr_->setCurrentControlValue(prev_cmd); + control_performance_core_ptr_->setOdomHistory(*msg); // k+1, k, k-1 + control_performance_core_ptr_->setSteeringStatus(prev_steering); + + if (!control_performance_core_ptr_->isDataReady()) { return; } + // Find the index of the next waypoint. + std::pair prev_closest_wp_pose_idx = + control_performance_core_ptr_->findClosestPrevWayPointIdx_path_direction(); - // If successful publish. - publishErrorMsg(*performanceVars); -} - -void ControlPerformanceAnalysisNode::publishErrorMsg( - const TargetPerformanceMsgVars & control_performance_vars) -{ - control_performance_analysis::ErrorStamped error_msgs = - createPerformanceMsgVars(control_performance_vars); + if (!prev_closest_wp_pose_idx.first) { + RCLCPP_ERROR(get_logger(), "Cannot find closest waypoint"); + return; + } - pub_error_msg_->publish(error_msgs); + // Compute control performance values. + if (control_performance_core_ptr_->calculateErrorVars()) { + pub_error_msg_->publish(control_performance_core_ptr_->error_vars); + } else { + RCLCPP_ERROR(get_logger(), "Cannot compute error vars ..."); + } + if (control_performance_core_ptr_->calculateDrivingVars()) { + control_performance_core_ptr_->driving_status_vars.controller_processing_time.header.stamp = + current_control_msg_ptr_->stamp; + control_performance_core_ptr_->driving_status_vars.controller_processing_time.data = + d_control_cmd_; + pub_driving_msg_->publish(control_performance_core_ptr_->driving_status_vars); + } else { + RCLCPP_ERROR(get_logger(), "Cannot compute driving vars ..."); + } + prev_traj = *current_trajectory_ptr_; + prev_cmd = *current_control_msg_ptr_; + prev_steering = *current_vec_steering_msg_ptr_; + current_odom_ptr_ = msg; + current_pose_ = self_pose_listener_.getCurrentPose(); } bool ControlPerformanceAnalysisNode::isDataReady() const { rclcpp::Clock clock{RCL_ROS_TIME}; - if (!current_pose_) { - RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_pose ..."); - return false; - } + // if (!current_pose_) { + // RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_pose ..."); + // return false; + // } if (!current_trajectory_ptr_) { RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for trajectory ... "); return false; } - if (!current_odom_ptr_) { - RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_odom ..."); - return false; - } + // if (!current_odom_ptr_) { + // RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_odom ..."); + // return false; + // } if (!current_control_msg_ptr_) { - RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_control_steering_val ..."); + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_control_cmd ..."); return false; } @@ -201,36 +217,6 @@ bool ControlPerformanceAnalysisNode::isDataReady() const * -> computePerformanceVars * */ -boost::optional -ControlPerformanceAnalysisNode::computeTargetPerformanceMsgVars() const -{ - // Set trajectory and current pose of controller_performance_core. - control_performance_core_ptr_->setCurrentWaypoints(*current_trajectory_ptr_); - control_performance_core_ptr_->setCurrentPose(current_pose_->pose); - control_performance_core_ptr_->setCurrentVelocities(current_odom_ptr_->twist.twist); - control_performance_core_ptr_->setCurrentControlValue(*current_control_msg_ptr_); - - // Find the index of the next waypoint. - std::pair prev_closest_wp_pose_idx = - control_performance_core_ptr_->findClosestPrevWayPointIdx_path_direction(); - - if (!prev_closest_wp_pose_idx.first) { - RCLCPP_ERROR(get_logger(), "Cannot find closest waypoint"); - return {}; - } - - // Compute control performance values. - const std::pair target_performance_vars = - control_performance_core_ptr_->getPerformanceVars(); - - if (!target_performance_vars.first) { - RCLCPP_ERROR(get_logger(), "Cannot compute control performance vars ..."); - return {}; - } - - return target_performance_vars.second; -} - bool ControlPerformanceAnalysisNode::isValidTrajectory(const Trajectory & traj) { bool check_condition = std::all_of(traj.points.cbegin(), traj.points.cend(), [](auto point) { diff --git a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp index ba4ae2d194701..ce11e10700487 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License.