From 763f61b03c041bedcf4b665fa19e4b6c977b9fa1 Mon Sep 17 00:00:00 2001 From: Berkay Date: Mon, 20 Jun 2022 17:06:16 +0300 Subject: [PATCH 1/6] feat(control_performance_analysis): monitor desired and current steering tire angle in driving monitor Signed-off-by: Berkay --- .../config/controller_monitor.xml | 325 +++++++++--------- .../control_performance_analysis_core.hpp | 5 +- .../msg/DrivingMonitorStamped.msg | 1 + .../src/control_performance_analysis_core.cpp | 163 +++++---- 4 files changed, 277 insertions(+), 217 deletions(-) diff --git a/control/control_performance_analysis/config/controller_monitor.xml b/control/control_performance_analysis/config/controller_monitor.xml index c4247c3a9d0a3..611caba37ee3a 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,42 +503,43 @@ 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..aaf6844f209c7 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,8 @@ 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..f6cf0543837d3 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -24,7 +24,8 @@ namespace control_performance_analysis { using geometry_msgs::msg::Quaternion; -ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() : wheelbase_{2.74} +ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() +: wheelbase_{2.74} { prev_target_vars_ = std::make_unique(); prev_driving_vars_ = std::make_unique(); @@ -102,23 +103,23 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint std::vector projection_distances_ds; auto f_projection_dist = [this](auto pose_1, auto pose_0) { - // Vector of intervals. - std::vector int_vec{ - pose_1.position.x - pose_0.position.x, pose_1.position.y - pose_0.position.y}; + // Vector of intervals. + std::vector int_vec{ + pose_1.position.x - pose_0.position.x, pose_1.position.y - pose_0.position.y}; - // Compute the magnitude of path interval vector - double ds_mag = std::hypot(int_vec[0], int_vec[1]); + // Compute the magnitude of path interval vector + double ds_mag = std::hypot(int_vec[0], int_vec[1]); - // Vector to vehicle from the origin waypoints. - std::vector vehicle_vec{ - this->current_vec_pose_ptr_->position.x - pose_0.position.x, - this->current_vec_pose_ptr_->position.y - pose_0.position.y}; + // Vector to vehicle from the origin waypoints. + std::vector vehicle_vec{ + 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 = - (int_vec[0] * vehicle_vec[0] + int_vec[1] * vehicle_vec[1]) / ds_mag; + double projection_distance_onto_interval = + (int_vec[0] * vehicle_vec[0] + int_vec[1] * vehicle_vec[1]) / ds_mag; - return projection_distance_onto_interval; - }; + return projection_distance_onto_interval; + }; // Fill the projection_distances vector. std::transform( @@ -128,8 +129,8 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint // Lambda function to replace negative numbers with a large number. auto fnc_check_if_negative = [](auto x) { - return x < 0 ? std::numeric_limits::max() : x; - }; + return x < 0 ? std::numeric_limits::max() : x; + }; std::vector projections_distances_all_positive; std::transform( @@ -150,9 +151,9 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); 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()); + (*idx_prev_wp_ <= length_of_trajectory)) ? + std::make_pair(true, *idx_prev_wp_) : + std::make_pair(false, std::numeric_limits::quiet_NaN()); } bool ControlPerformanceAnalysisCore::isDataReady() const @@ -202,10 +203,11 @@ 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; } @@ -258,16 +260,16 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() 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; + 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; + 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; + 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; + 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 @@ -281,7 +283,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() 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; + *this->interpolated_velocity_ptr_ * curvature_est; double && lateral_acceleration_error = -curvature_est * *interpolated_acceleration_ptr_ * longitudinal_error - @@ -372,6 +374,15 @@ 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( @@ -384,11 +395,11 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() // 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; + 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)); + rclcpp::Time(odom_history_ptr_->at(odom_size - odom_interval_ - 2).header.stamp)); double dt = duration.seconds(); @@ -400,7 +411,7 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() // Calculate lateral jerk double d_lateral_jerk = driving_status_vars.lateral_acceleration.data - - prev_driving_vars_->lateral_acceleration.data; + prev_driving_vars_->lateral_acceleration.data; // We already know the delta time from above. same as longitudinal acceleration @@ -412,10 +423,10 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() 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; + 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)); + 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( @@ -490,12 +501,12 @@ void ControlPerformanceAnalysisCore::findCurveRefIdx() } auto fun_distance_cond = [this](auto pose_t) { - double dist = std::hypot( - pose_t.position.x - this->interpolated_pose_ptr_->position.x, - pose_t.position.y - this->interpolated_pose_ptr_->position.y); + double dist = std::hypot( + pose_t.position.x - this->interpolated_pose_ptr_->position.x, + pose_t.position.y - this->interpolated_pose_ptr_->position.y); - return dist > wheelbase_; - }; + return dist > wheelbase_; + }; auto it = std::find_if( current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), @@ -551,10 +562,10 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() // 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; + 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; + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; double && delta_psi_prev2next = utils::angleDistance(next_yaw, prev_yaw); double && d_vel_prev2next = next_velocity - prev_velocity; @@ -588,6 +599,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,21 +623,49 @@ 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) { 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; + 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; + 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; + 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); + 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_); } @@ -634,33 +674,38 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() 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; + 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; + 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; + 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_); + 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); + 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() @@ -691,9 +736,9 @@ double ControlPerformanceAnalysisCore::estimateCurvature() std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); auto num_of_forward_indices = num_of_back_indices; - int32_t loc_of_forward_idx = (*idx_curve_ref_wp_ + num_of_forward_indices > max_idx) - ? max_idx - 1 - : *idx_curve_ref_wp_ + num_of_forward_indices - 1; + int32_t loc_of_forward_idx = (*idx_curve_ref_wp_ + num_of_forward_indices > max_idx) ? + max_idx - 1 : + *idx_curve_ref_wp_ + num_of_forward_indices - 1; // We have three indices of the three trajectory poses. // We compute a curvature estimate from these points. @@ -730,12 +775,12 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() double look_ahead_distance_pp = std::max(wheelbase_, 2 * Vx); auto fun_distance_cond = [this, &look_ahead_distance_pp](auto pose_t) { - double dist = std::hypot( - pose_t.position.x - this->interpolated_pose_ptr_->position.x, - pose_t.position.y - this->interpolated_pose_ptr_->position.y); + double dist = std::hypot( + pose_t.position.x - this->interpolated_pose_ptr_->position.x, + pose_t.position.y - this->interpolated_pose_ptr_->position.y); - return dist > look_ahead_distance_pp; - }; + return dist > look_ahead_distance_pp; + }; auto it = std::find_if( current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), From 1bb84a1d45bc213e759c26679dc0dd7509706df3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 20 Jun 2022 14:12:33 +0000 Subject: [PATCH 2/6] ci(pre-commit): autofix --- .../config/controller_monitor.xml | 1 - .../control_performance_analysis_core.hpp | 1 - .../src/control_performance_analysis_core.cpp | 139 ++++++++---------- 3 files changed, 64 insertions(+), 77 deletions(-) diff --git a/control/control_performance_analysis/config/controller_monitor.xml b/control/control_performance_analysis/config/controller_monitor.xml index 611caba37ee3a..e1a3bbc2bce80 100644 --- a/control/control_performance_analysis/config/controller_monitor.xml +++ b/control/control_performance_analysis/config/controller_monitor.xml @@ -542,4 +542,3 @@ return sum - 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 aaf6844f209c7..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 @@ -118,7 +118,6 @@ class ControlPerformanceAnalysisCore 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(); double const contR{10.0}; // Control weight in LQR 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 f6cf0543837d3..866fe58d57749 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -24,8 +24,7 @@ namespace control_performance_analysis { using geometry_msgs::msg::Quaternion; -ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() -: wheelbase_{2.74} +ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() : wheelbase_{2.74} { prev_target_vars_ = std::make_unique(); prev_driving_vars_ = std::make_unique(); @@ -103,23 +102,23 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint std::vector projection_distances_ds; auto f_projection_dist = [this](auto pose_1, auto pose_0) { - // Vector of intervals. - std::vector int_vec{ - pose_1.position.x - pose_0.position.x, pose_1.position.y - pose_0.position.y}; + // Vector of intervals. + std::vector int_vec{ + pose_1.position.x - pose_0.position.x, pose_1.position.y - pose_0.position.y}; - // Compute the magnitude of path interval vector - double ds_mag = std::hypot(int_vec[0], int_vec[1]); + // Compute the magnitude of path interval vector + double ds_mag = std::hypot(int_vec[0], int_vec[1]); - // Vector to vehicle from the origin waypoints. - std::vector vehicle_vec{ - this->current_vec_pose_ptr_->position.x - pose_0.position.x, - this->current_vec_pose_ptr_->position.y - pose_0.position.y}; + // Vector to vehicle from the origin waypoints. + std::vector vehicle_vec{ + 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 = - (int_vec[0] * vehicle_vec[0] + int_vec[1] * vehicle_vec[1]) / ds_mag; + double projection_distance_onto_interval = + (int_vec[0] * vehicle_vec[0] + int_vec[1] * vehicle_vec[1]) / ds_mag; - return projection_distance_onto_interval; - }; + return projection_distance_onto_interval; + }; // Fill the projection_distances vector. std::transform( @@ -129,8 +128,8 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint // Lambda function to replace negative numbers with a large number. auto fnc_check_if_negative = [](auto x) { - return x < 0 ? std::numeric_limits::max() : x; - }; + return x < 0 ? std::numeric_limits::max() : x; + }; std::vector projections_distances_all_positive; std::transform( @@ -151,9 +150,9 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); 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()); + (*idx_prev_wp_ <= length_of_trajectory)) + ? std::make_pair(true, *idx_prev_wp_) + : std::make_pair(false, std::numeric_limits::quiet_NaN()); } bool ControlPerformanceAnalysisCore::isDataReady() const @@ -203,8 +202,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() if ( !pair_pose_interp_wp_.first || !interpolated_pose_ptr_ || !interpolated_velocity_ptr_ || - !interpolated_acceleration_ptr_ || !interpolated_steering_angle_ptr_) - { + !interpolated_acceleration_ptr_ || !interpolated_steering_angle_ptr_) { RCLCPP_WARN_THROTTLE( logger_, clock_, 1000, "Cannot get interpolated pose, velocity, acceleration, and steering into control_performance " @@ -260,16 +258,16 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() 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; + 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; + 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; + 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; + 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 @@ -283,7 +281,7 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() 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; + *this->interpolated_velocity_ptr_ * curvature_est; double && lateral_acceleration_error = -curvature_est * *interpolated_acceleration_ptr_ * longitudinal_error - @@ -374,7 +372,6 @@ 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_) { @@ -395,11 +392,11 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() // 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; + 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)); + rclcpp::Time(odom_history_ptr_->at(odom_size - odom_interval_ - 2).header.stamp)); double dt = duration.seconds(); @@ -411,7 +408,7 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() // Calculate lateral jerk double d_lateral_jerk = driving_status_vars.lateral_acceleration.data - - prev_driving_vars_->lateral_acceleration.data; + prev_driving_vars_->lateral_acceleration.data; // We already know the delta time from above. same as longitudinal acceleration @@ -423,10 +420,10 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() 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; + 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)); + 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( @@ -501,12 +498,12 @@ void ControlPerformanceAnalysisCore::findCurveRefIdx() } auto fun_distance_cond = [this](auto pose_t) { - double dist = std::hypot( - pose_t.position.x - this->interpolated_pose_ptr_->position.x, - pose_t.position.y - this->interpolated_pose_ptr_->position.y); + double dist = std::hypot( + pose_t.position.x - this->interpolated_pose_ptr_->position.x, + pose_t.position.y - this->interpolated_pose_ptr_->position.y); - return dist > wheelbase_; - }; + return dist > wheelbase_; + }; auto it = std::find_if( current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), @@ -562,10 +559,10 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() // 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; + 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; + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; double && delta_psi_prev2next = utils::angleDistance(next_yaw, prev_yaw); double && d_vel_prev2next = next_velocity - prev_velocity; @@ -625,47 +622,41 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() /* 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; + 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; + 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))); + 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))); + (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) { 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; + 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; + 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; + 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); + 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_); } @@ -674,15 +665,15 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() 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; + 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; + 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; + 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_); + 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_); } @@ -690,8 +681,7 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() double && interp_acceleration = prev_wp_acc + ratio_t * d_acc_prev2next; setInterpolatedVars( - interpolated_pose, interp_velocity, interp_acceleration, - interp_steering_angle); + interpolated_pose, interp_velocity, interp_acceleration, interp_steering_angle); return std::make_pair(true, interpolated_pose); } @@ -705,7 +695,6 @@ void ControlPerformanceAnalysisCore::setInterpolatedVars( 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() @@ -736,9 +725,9 @@ double ControlPerformanceAnalysisCore::estimateCurvature() std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); auto num_of_forward_indices = num_of_back_indices; - int32_t loc_of_forward_idx = (*idx_curve_ref_wp_ + num_of_forward_indices > max_idx) ? - max_idx - 1 : - *idx_curve_ref_wp_ + num_of_forward_indices - 1; + int32_t loc_of_forward_idx = (*idx_curve_ref_wp_ + num_of_forward_indices > max_idx) + ? max_idx - 1 + : *idx_curve_ref_wp_ + num_of_forward_indices - 1; // We have three indices of the three trajectory poses. // We compute a curvature estimate from these points. @@ -775,12 +764,12 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() double look_ahead_distance_pp = std::max(wheelbase_, 2 * Vx); auto fun_distance_cond = [this, &look_ahead_distance_pp](auto pose_t) { - double dist = std::hypot( - pose_t.position.x - this->interpolated_pose_ptr_->position.x, - pose_t.position.y - this->interpolated_pose_ptr_->position.y); + double dist = std::hypot( + pose_t.position.x - this->interpolated_pose_ptr_->position.x, + pose_t.position.y - this->interpolated_pose_ptr_->position.y); - return dist > look_ahead_distance_pp; - }; + return dist > look_ahead_distance_pp; + }; auto it = std::find_if( current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), From 78fc9c06790e82e9ca219b584a64274d796bf5ab Mon Sep 17 00:00:00 2001 From: Berkay Date: Mon, 20 Jun 2022 17:19:17 +0300 Subject: [PATCH 3/6] update readme Signed-off-by: Berkay --- control/control_performance_analysis/README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index bb698f9c9d497..49791068f5379 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -43,11 +43,12 @@ Error acceleration calculations are made based on the velocity calculations abov #### 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] | +| `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 From d9b2c0fb2b0b742e8dac4bc7bdbd8359064f774e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 20 Jun 2022 14:22:13 +0000 Subject: [PATCH 4/6] ci(pre-commit): autofix --- control/control_performance_analysis/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 49791068f5379..18348742012a3 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -43,7 +43,7 @@ Error acceleration calculations are made based on the velocity calculations abov #### 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] | From d6217fc450d949b861ea5d2355229988e452b39a Mon Sep 17 00:00:00 2001 From: Berkay Date: Tue, 21 Jun 2022 14:46:26 +0300 Subject: [PATCH 5/6] update lpf for steering monitor Signed-off-by: Berkay --- .../src/control_performance_analysis_core.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 866fe58d57749..cf56b1ad94f7d 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -452,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_ = From 2b0e07a2f0de918555b2702538c75ac827ed8ba8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 21 Jun 2022 11:47:45 +0000 Subject: [PATCH 6/6] ci(pre-commit): autofix --- .../src/control_performance_analysis_core.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 cf56b1ad94f7d..6dd8e8878db58 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -454,8 +454,8 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() (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; + lpf_gain_ * prev_driving_vars_->desired_steering_angle.data + + (1 - lpf_gain_) * driving_status_vars.desired_steering_angle.data; } prev_driving_vars_ =