From 1210074ca2a79997876061a89cdf0ad82f514014 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Fri, 15 Sep 2023 18:19:29 +0300 Subject: [PATCH] kalan --- .../longitudinal_controller_utils.hpp | 3 +- .../src/longitudinal_controller_utils.cpp | 64 +++++++++++-------- .../src/pid_longitudinal_controller.cpp | 41 ++++++++++-- 3 files changed, 76 insertions(+), 32 deletions(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 893addb0c72f2..026ee83299e04 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -161,7 +161,8 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, const autoware_auto_planning_msgs::msg::Trajectory & trajectory); +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point); } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller - #endif // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 0cc0070119fb3..9c516747f5dc6 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -58,11 +58,23 @@ bool isValidTrajectory(const Trajectory & traj) double calcStopDistance( const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw) { - const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points); - - const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( traj.points, current_pose, max_dist, max_yaw); + const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points); + const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1; + // if (end_idx == traj.points.size() - 1 && 1 >= int(end_idx - seg_idx)) { + // // Check the ego in front of the last point of trajectory or not + // const auto yaw = tier4_autoware_utils::getRPY(current_pose).z; + // const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + // const Eigen::Vector2d vehicle_to_end_vec( + // traj.points.back().pose.position.x - current_pose.position.x, + // traj.points.back().pose.position.y - current_pose.position.y); + // if (base_pose_vec.dot(vehicle_to_end_vec) < 0.0) { + // return -1.0 * + // tier4_autoware_utils::calcDistance3d(current_pose, traj.points.at(end_idx).pose); + // } + // return tier4_autoware_utils::calcDistance3d(current_pose, traj.points.at(end_idx).pose); + // } const double signed_length_on_traj = motion_utils::calcSignedArcLength( traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position, std::min(end_idx, traj.points.size() - 2)); @@ -96,29 +108,7 @@ double getPitchByTraj( TrajectoryPoint point_after_distance = trajectory.points.back(); point_after_distance.pose = pose_after_distance; - for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance2d( - trajectory.points.at(nearest_idx), trajectory.points.at(i)); - if (dist > wheel_base) { - // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i)); - } - } - - // close to goal - for (size_t i = trajectory.points.size() - 1; i > 0; --i) { - const double dist = - tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i)); - - if (dist > wheel_base) { - // calculate pitch from trajectory - // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) - return calcElevationAngle(trajectory.points.at(i), trajectory.points.back()); - } - } - - // calculate pitch from trajectory between the beginning and end of trajectory - return calcElevationAngle(trajectory.points.at(0), trajectory.points.back()); + return calcElevationAngle(trajectory.points.at(nearest_idx), point_after_distance); } double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) @@ -216,5 +206,27 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( } return p; } + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point) +{ + tf2::Transform map2goal; + tf2::fromMsg(goal_point.pose, map2goal); + tf2::Transform local_extend_point; + local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); + tf2::Quaternion q; + q.setRPY(0, 0, 0); + local_extend_point.setRotation(q); + const auto map2extend_point = map2goal * local_extend_point; + geometry_msgs::msg::Pose extend_pose; + tf2::toMsg(map2extend_point, extend_pose); + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extend_trajectory_point; +} + } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index cf744faf6265f..01f744c0b03ee 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -221,8 +221,12 @@ void PidLongitudinalController::setTrajectory( RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored."); return; } - + // If the vehicle pass the last point of trajectory, it causes errors on control_data calculation. + // To handle this, we add a virtual point after the last point. + constexpr double virtual_point_distance = 5.0; m_trajectory = msg; + m_trajectory.points.push_back(longitudinal_utils::getExtendTrajectoryPoint( + virtual_point_distance, m_trajectory.points.back())); } rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback( @@ -870,6 +874,14 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop return output_motion; } + if (control_data.stop_dist < 0.0) { + const auto & p = m_stopped_state_params; + output_motion.vel = p.vel; + output_motion.acc = longitudinal_utils::applyDiffLimitFilter( + p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); + return output_motion; + } + double min_acc_before_stop = std::numeric_limits::max(); size_t min_acc_idx = std::numeric_limits::max(); for (int i = static_cast(*stop_idx); i >= 0; --i) { @@ -980,10 +992,29 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont // deviation will be bigger. constexpr double ff_scale_max = 2.0; // for safety constexpr double ff_scale_min = 0.5; // for safety - const double ff_scale = - std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max); - const double ff_acc = target_motion.acc * ff_scale; - + const double ff_scale = std::clamp( + 1.0 + (abs(current_target_vel_abs - abs(control_data.current_motion.vel)) / + std::max(current_target_vel_abs, 0.1)), + ff_scale_min, ff_scale_max); + + // Feedforward acceleration: + const double tmp_ff_acc = + control_data.target_idx == control_data.interpolated_traj.points.size() - 1 || + control_data.current_motion.vel < 0.01 + ? control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 + : control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * + (std::max( + tier4_autoware_utils::calcDistance2d( + control_data.interpolated_traj.points.at(control_data.target_idx + 1).pose, + control_data.interpolated_traj.points.at(control_data.target_idx).pose), + std::numeric_limits::min()) / + std::max( + tier4_autoware_utils::calcDistance3d( + control_data.interpolated_traj.points.at(control_data.target_idx + 1).pose, + control_data.interpolated_traj.points.at(control_data.target_idx).pose), + std::numeric_limits::min())); + + const double ff_acc = tmp_ff_acc * ff_scale; const double feedback_acc = ff_acc + pid_acc; m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PID_APPLIED, feedback_acc);