diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index bb698f9c9d497..18348742012a3 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -48,6 +48,7 @@ Error acceleration calculations are made based on the velocity calculations abov | `longitudinal_jerk` | float | [m / s^3] | | `lateral_acceleration` | float | [m / s^2] | | `lateral_jerk` | float | [m / s^3] | +| `desired_steering_angle` | float | [rad] | | `controller_processing_time` | float | Timestamp between last two control command messages [ms] | #### control_performance_analysis::msg::ErrorStamped @@ -87,6 +88,7 @@ Error acceleration calculations are made based on the velocity calculations abov - After import the layout, please specify the topics that are listed below. > - /localization/kinematic_state +> - /vehicle/status/steering_status > - /control_performance/driving_status > - /control_performance/performance_vars diff --git a/control/control_performance_analysis/config/controller_monitor.xml b/control/control_performance_analysis/config/controller_monitor.xml index c4247c3a9d0a3..e1a3bbc2bce80 100644 --- a/control/control_performance_analysis/config/controller_monitor.xml +++ b/control/control_performance_analysis/config/controller_monitor.xml @@ -1,98 +1,108 @@ - - + + - - + + - - + + - - + + - + - - + + - - + + - - - - - - - + + + + + + + + + + + + + + + + + - + - - - + + + - - + + - - + + - + - - + + - - + + - + - - + + - - + + @@ -102,53 +112,53 @@ - + - - - + + + - - + + - - + + - + - - + + - - + + - + - - + + - - + + @@ -158,27 +168,27 @@ - + - + - - + + - + - - + + - - + + @@ -187,50 +197,50 @@ - + - - + + - - + + - - + + - - + + - + - - + + - - + + - - + + @@ -239,50 +249,50 @@ - + - - + + - - + + - - + + - - + + - + - - + + - - + + - - + + @@ -291,28 +301,28 @@ - + - - + + - - + + - - + + - - + + @@ -320,28 +330,28 @@ - + - - + + - - + + - - + + - - + + @@ -349,27 +359,27 @@ - + - + - - + + - + - + - + @@ -402,13 +412,6 @@ - - sum = 0 - sum = sum + math.abs(value) - -return sum - /control_performance/performance_vars/error/longitudinal_error_acceleration - sum = 0 sum = sum + math.abs(value) @@ -444,47 +447,54 @@ return sum 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 + /control_performance/performance_vars/error/lateral_error_acceleration - + sum = 0 sum = sum + math.abs(value) return sum - /control_performance/performance_vars/error/longitudinal_error_velocity + /control_performance/performance_vars/error/lateral_error_velocity - + sum = 0 - sum = sum + math.sqrt(value*value) + sum = sum + math.abs(value) return sum - /control_performance/performance_vars/error/lateral_error_velocity + /control_performance/performance_vars/error/heading_error_velocity - + sum = 0 - sum = sum + math.sqrt(value*value) + sum = sum + math.abs(value) return sum - /control_performance/performance_vars/error/lateral_error_acceleration + /control_performance/performance_vars/error/longitudinal_error_acceleration - + sum = 0 - sum = sum + math.sqrt(value*value) + sum = sum + math.abs(value) return sum - /control_performance/performance_vars/error/longitudinal_error + /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.sqrt(value*value) return sum - /control_performance/performance_vars/error/longitudinal_error_acceleration + /control_performance/performance_vars/error/tracking_curvature_discontinuity_ability sum = 0 @@ -493,40 +503,40 @@ return sum 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 + /control_performance/performance_vars/error/longitudinal_error_acceleration - + sum = 0 - sum = sum + math.abs(value) + sum = sum + math.sqrt(value*value) return sum - /control_performance/performance_vars/error/tracking_curvature_discontinuity_ability + /control_performance/performance_vars/error/longitudinal_error - + sum = 0 - sum = sum + math.abs(value) + sum = sum + math.sqrt(value*value) return sum - /control_performance/performance_vars/error/lateral_error + /control_performance/performance_vars/error/lateral_error_velocity - + sum = 0 sum = sum + math.abs(value) return sum - /control_performance/performance_vars/error/heading_error_velocity + /control_performance/performance_vars/error/longitudinal_error_velocity - + sum = 0 - sum = sum + math.abs(value) + sum = sum + math.sqrt(value*value) return sum - /control_performance/performance_vars/error/lateral_error_velocity + /control_performance/performance_vars/error/lateral_error 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 e74c7e215fae8..ea8dcd20e7960 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 @@ -64,7 +64,8 @@ class ControlPerformanceAnalysisCore void setCurrentWaypoints(const Trajectory & trajectory); void setCurrentControlValue(const AckermannControlCommand & msg); void setInterpolatedVars( - Pose & interpolated_pose, double & interpolated_velocity, double & interpolated_acceleration); + Pose & interpolated_pose, double & interpolated_velocity, double & interpolated_acceleration, + double & interpolated_steering_angle); void setOdomHistory(const Odometry & odom); void setSteeringStatus(const SteeringReport & steering); @@ -115,6 +116,7 @@ class ControlPerformanceAnalysisCore std::shared_ptr interpolated_pose_ptr_; std::shared_ptr interpolated_velocity_ptr_; std::shared_ptr interpolated_acceleration_ptr_; + std::shared_ptr interpolated_steering_angle_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(); diff --git a/control/control_performance_analysis/msg/DrivingMonitorStamped.msg b/control/control_performance_analysis/msg/DrivingMonitorStamped.msg index c53592bc889ed..2dea0b5e36b8f 100644 --- a/control/control_performance_analysis/msg/DrivingMonitorStamped.msg +++ b/control/control_performance_analysis/msg/DrivingMonitorStamped.msg @@ -2,4 +2,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 desired_steering_angle control_performance_analysis/FloatStamped controller_processing_time 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 3fe0752905cc4..6dd8e8878db58 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -202,10 +202,10 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() if ( !pair_pose_interp_wp_.first || !interpolated_pose_ptr_ || !interpolated_velocity_ptr_ || - !interpolated_acceleration_ptr_) { + !interpolated_acceleration_ptr_ || !interpolated_steering_angle_ptr_) { RCLCPP_WARN_THROTTLE( logger_, clock_, 1000, - "Cannot get interpolated pose, velocity, and acceleration into control_performance " + "Cannot get interpolated pose, velocity, acceleration, and steering into control_performance " "algorithm"); return false; } @@ -372,6 +372,14 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() const uint odom_size = odom_history_ptr_->size(); if (odom_history_ptr_->at(odom_size - 1).header.stamp != last_odom_header.stamp) { + // Add desired steering angle + + if (interpolated_steering_angle_ptr_) { + driving_status_vars.desired_steering_angle.header = + odom_history_ptr_->at(odom_size - 1).header; + driving_status_vars.desired_steering_angle.data = *interpolated_steering_angle_ptr_; + } + // Calculate lateral acceleration driving_status_vars.lateral_acceleration.header.set__stamp( @@ -444,6 +452,10 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() driving_status_vars.controller_processing_time.data = lpf_gain_ * prev_driving_vars_->controller_processing_time.data + (1 - lpf_gain_) * driving_status_vars.controller_processing_time.data; + + driving_status_vars.desired_steering_angle.data = + lpf_gain_ * prev_driving_vars_->desired_steering_angle.data + + (1 - lpf_gain_) * driving_status_vars.desired_steering_angle.data; } prev_driving_vars_ = @@ -588,6 +600,7 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() double && distance_p02p_interp = (dx_prev2next * dx_prev2vehicle + dy_prev2next * dy_prev2vehicle) / distance_p02p1; + double && distance_p_interp2p1 = distance_p02p1 - distance_p02p_interp; /* * We use the following linear interpolation * pi = p0 + ratio_t * (p1 - p0) @@ -611,6 +624,28 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() Quaternion && orient_msg = utils::createOrientationMsgFromYaw(interp_yaw_angle); interpolated_pose.orientation = orient_msg; + /* interpolated steering calculation */ + + double interp_steering_angle = 0.0; + + if (static_cast(*idx_next_wp_ + 1) < current_waypoints_ptr_->poses.size()) { + double && dx_p1_p2 = current_waypoints_ptr_->poses.at(*idx_next_wp_ + 1).position.x - + current_waypoints_ptr_->poses.at(*idx_next_wp_).position.x; + double && dy_p1_p2 = current_waypoints_ptr_->poses.at(*idx_next_wp_ + 1).position.y - + current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y; + double && distance_p1_p2 = std::hypot(dx_p1_p2, dy_p1_p2); + double && prev_steering = + static_cast(std::atan((next_yaw - prev_yaw) * wheelbase_ / (distance_p02p1))); + double && next_steering = static_cast(std::atan( + (tf2::getYaw(current_waypoints_ptr_->poses.at(*idx_next_wp_ + 1).orientation) - next_yaw) * + wheelbase_ / (distance_p1_p2))); + interp_steering_angle = prev_steering + ratio_t * (next_steering - prev_steering); + } else { + interp_steering_angle = static_cast(std::atan( + (next_yaw - tf2::getYaw(interpolated_pose.orientation)) * wheelbase_ / + (distance_p_interp2p1))); + } + /* interpolated acceleration calculation */ if (*idx_prev_wp_ == 0 || *idx_prev_wp_ == total_num_of_waypoints_in_traj - 1) { @@ -649,18 +684,21 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() 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); + setInterpolatedVars( + interpolated_pose, interp_velocity, interp_acceleration, interp_steering_angle); return std::make_pair(true, interpolated_pose); } // Sets interpolated waypoint_ptr_. void ControlPerformanceAnalysisCore::setInterpolatedVars( - Pose & interpolated_pose, double & interpolated_velocity, double & interpolated_acceleration) + Pose & interpolated_pose, double & interpolated_velocity, double & interpolated_acceleration, + double & interpolated_steering_angle) { interpolated_pose_ptr_ = std::make_shared(interpolated_pose); interpolated_velocity_ptr_ = std::make_shared(interpolated_velocity); interpolated_acceleration_ptr_ = std::make_shared(interpolated_acceleration); + interpolated_steering_angle_ptr_ = std::make_shared(interpolated_steering_angle); } double ControlPerformanceAnalysisCore::estimateCurvature()