From 2fd8ffc4b8930fbcc75d03b9cdd13bf84f9d5bd5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 19 Aug 2022 13:41:09 +0900 Subject: [PATCH] fix(velocity_controller, mpc_follower): use common ego nearest search (#1590) * fix(trajectory_follower): use common ego nearest search Signed-off-by: Takayuki Murooka * removed calcNearestIndex Signed-off-by: Takayuki Murooka * add nearest search param Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * Update launch/tier4_control_launch/launch/control.launch.py * update test fil Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../longitudinal_controller_utils.hpp | 12 ++- .../include/trajectory_follower/mpc.hpp | 3 + .../mpc_lateral_controller.hpp | 4 + .../include/trajectory_follower/mpc_utils.hpp | 26 ++---- .../pid_longitudinal_controller.hpp | 6 +- .../src/longitudinal_controller_utils.cpp | 29 ++----- control/trajectory_follower/src/mpc.cpp | 20 +++-- .../src/mpc_lateral_controller.cpp | 28 +++++-- control/trajectory_follower/src/mpc_utils.cpp | 83 ++++--------------- .../src/pid_longitudinal_controller.cpp | 63 +++++++------- .../test/test_mpc_utils.cpp | 22 ----- .../trajectory_follower_nodes/CMakeLists.txt | 3 +- .../param/test_nearest_search.param.yaml | 5 ++ .../test/test_controller_node.cpp | 3 +- .../test/test_lateral_controller_node.cpp | 3 +- .../test_longitudinal_controller_node.cpp | 3 +- .../config/common/nearest_search.param.yaml | 5 ++ .../launch/control.launch.py | 12 ++- 18 files changed, 142 insertions(+), 188 deletions(-) create mode 100644 control/trajectory_follower_nodes/param/test_nearest_search.param.yaml create mode 100644 launch/tier4_control_launch/config/common/nearest_search.param.yaml diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 75f70defc6420..1a18f6c166f14 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -112,19 +112,17 @@ TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( const T & points, const Pose & pose, const float64_t max_dist, const float64_t max_yaw) { TrajectoryPoint interpolated_point; - auto seg_idx = motion_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw); - if (!seg_idx) { // if not fund idx - seg_idx = motion_utils::findNearestSegmentIndex(points, pose); - } + const size_t seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(points, pose, max_dist, max_yaw); const float64_t len_to_interpolated = - motion_utils::calcLongitudinalOffsetToSegment(points, *seg_idx, pose.position); + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); const float64_t len_segment = - trajectory_common::calcSignedArcLength(points, *seg_idx, *seg_idx + 1); + trajectory_common::calcSignedArcLength(points, seg_idx, seg_idx + 1); const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); { - const size_t i = *seg_idx; + const size_t i = seg_idx; interpolated_point.pose.position.x = motion_common::interpolate( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index e40d82d6b3b42..4b7442a186f0b 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp @@ -365,6 +365,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC /* parameters for path smoothing */ //!< @brief flag to use predicted steer, not measured steer. bool8_t m_use_steer_prediction; + //!< @brief parameters for nearest index search + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; /** * @brief constructor diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index e7155e8dcf56b..bfbf0d4834c13 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -138,6 +138,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME}; tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; + // ego nearest index search + double m_ego_nearest_dist_threshold; + double m_ego_nearest_yaw_threshold; + //!< initialize timer to work in real, simulation, and replay void initTimer(float64_t period_s); /** diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp index 02ae209e9b828..83f152f3e4df1 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -176,27 +176,11 @@ TRAJECTORY_FOLLOWER_PUBLIC std::vector calcTrajectoryCurvature( TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcNearestPoseInterp( const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time, - const rclcpp::Logger & logger, rclcpp::Clock & clock); -/** - * @brief calculate the index of the trajectory point nearest to the given pose - * @param [in] traj trajectory to search for the point nearest to the pose - * @param [in] self_pose pose for which to search the nearest trajectory point - * @return index of the input trajectory nearest to the pose - */ -TRAJECTORY_FOLLOWER_PUBLIC int64_t -calcNearestIndex(const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose); -/** - * @brief calculate the index of the trajectory point nearest to the given pose - * @param [in] traj trajectory to search for the point nearest to the pose - * @param [in] self_pose pose for which to search the nearest trajectory point - * @return index of the input trajectory nearest to the pose - */ -TRAJECTORY_FOLLOWER_PUBLIC int64_t calcNearestIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & self_pose); -/** - * @brief calculate distance to stopped point - */ + const double max_dist, const double max_yaw, const rclcpp::Logger & logger, + rclcpp::Clock & clock); +// /** +// * @brief calculate distance to stopped point +// */ TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int64_t origin); } // namespace MPCUtils diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 3bd3f38871a53..31efe59c64b18 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -186,6 +186,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal float64_t m_max_pitch_rad; float64_t m_min_pitch_rad; + // ego nearest index search + double m_ego_nearest_dist_threshold; + double m_ego_nearest_yaw_threshold; + // 1st order lowpass filter for acceleration std::shared_ptr m_lpf_acc{nullptr}; @@ -331,7 +335,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal */ autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue( const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const; + const geometry_msgs::msg::Pose & pose) const; /** * @brief calculate predicted velocity after time delay based on past control commands diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 3269c7766c645..55060d1b9bc7f 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -69,28 +69,17 @@ float64_t calcStopDistance( const std::experimental::optional stop_idx_opt = trajectory_common::searchZeroVelocityIndex(traj.points); - auto seg_idx = - motion_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw); - if (!seg_idx) { // if not fund idx - seg_idx = motion_utils::findNearestSegmentIndex(traj.points, current_pose); - } - const float64_t signed_length_src_offset = - motion_utils::calcLongitudinalOffsetToSegment(traj.points, *seg_idx, current_pose.position); - - if (std::isnan(signed_length_src_offset)) { + 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 float64_t 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)); + + if (std::isnan(signed_length_on_traj)) { return 0.0; } - - // If no zero velocity point, return the length between current_pose to the end of trajectory. - if (!stop_idx_opt) { - float64_t signed_length_on_traj = - motion_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1); - return signed_length_on_traj - signed_length_src_offset; - } - - float64_t signed_length_on_traj = - motion_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt); - return signed_length_on_traj - signed_length_src_offset; + return signed_length_on_traj; } float64_t getPitchByPose(const Quaternion & quaternion_msg) diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index 31142b0ee1324..a281a226a58cb 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -264,8 +264,8 @@ bool8_t MPC::getData( static constexpr auto duration = 5000 /*ms*/; size_t nearest_idx; if (!trajectory_follower::MPCUtils::calcNearestPoseInterp( - traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), m_logger, - *m_clock)) { + traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), + ego_nearest_dist_threshold, ego_nearest_yaw_threshold, m_logger, *m_clock)) { // reset previous MPC result // Note: When a large deviation from the trajectory occurs, the optimization stops and // the vehicle will return to the path by re-planning the trajectory or external operation. @@ -490,11 +490,16 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( const trajectory_follower::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose, const float64_t v0) const { - int64_t nearest_idx = trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose); - if (nearest_idx < 0) { + autoware_auto_planning_msgs::msg::Trajectory autoware_traj; + autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( + input, autoware_traj); + if (autoware_traj.points.empty()) { return input; } + const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + const float64_t acc_lim = m_param.acceleration_limit; const float64_t tau = m_param.velocity_time_constant; @@ -783,8 +788,11 @@ float64_t MPC::getPredictionDeletaTime( const geometry_msgs::msg::Pose & current_pose) const { // Calculate the time min_prediction_length ahead from current_pose - const size_t nearest_idx = - static_cast(trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose)); + autoware_auto_planning_msgs::msg::Trajectory autoware_traj; + autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory( + input, autoware_traj); + const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); float64_t sum_dist = 0; const float64_t target_time = [&]() { const float64_t t_ext = diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index faa9d4eb71b18..8dd198bccb8ed 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -14,6 +14,7 @@ #include "trajectory_follower/mpc_lateral_controller.hpp" +#include "motion_utils/motion_utils.hpp" #include "tf2_ros/create_timer_ros.h" #include @@ -140,6 +141,18 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); } + // ego nearest index search + m_ego_nearest_dist_threshold = + node_->has_parameter("ego_nearest_dist_threshold") + ? node_->get_parameter("ego_nearest_dist_threshold").as_double() + : node_->declare_parameter("ego_nearest_dist_threshold"); // [m] + m_ego_nearest_yaw_threshold = + node_->has_parameter("ego_nearest_yaw_threshold") + ? node_->get_parameter("ego_nearest_yaw_threshold").as_double() + : node_->declare_parameter("ego_nearest_yaw_threshold"); // [rad] + m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; + m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; + m_pub_predicted_traj = node_->create_publisher( "~/output/predicted_trajectory", 1); m_pub_diagnostic = @@ -356,17 +369,18 @@ MpcLateralController::getInitialControlCommand() const bool8_t MpcLateralController::isStoppedState() const { + // If the nearest index is not found, return false + if (m_current_trajectory_ptr->points.empty()) { + return false; + } + // Note: This function used to take into account the distance to the stop line // for the stop state judgement. However, it has been removed since the steering // control was turned off when approaching/exceeding the stop line on a curve or // emergency stop situation and it caused large tracking error. - const int64_t nearest = trajectory_follower::MPCUtils::calcNearestIndex( - *m_current_trajectory_ptr, m_current_pose_ptr->pose); - - // If the nearest index is not found, return false - if (nearest < 0) { - return false; - } + const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints( + m_current_trajectory_ptr->points, m_current_pose_ptr->pose, m_ego_nearest_dist_threshold, + m_ego_nearest_yaw_threshold); const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x; const float64_t target_vel = diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index 9f91c8feedc31..a56dc36a007de 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -14,6 +14,8 @@ #include "trajectory_follower/mpc_utils.hpp" +#include "motion_utils/motion_utils.hpp" + #include #include #include @@ -296,83 +298,26 @@ void dynamicSmoothingVelocity( calcMPCTrajectoryTime(traj); } -int64_t calcNearestIndex(const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose) -{ - if (traj.empty()) { - return -1; - } - const float64_t my_yaw = ::motion::motion_common::to_angle(self_pose.orientation); - int64_t nearest_idx = -1; - float64_t min_dist_squared = std::numeric_limits::max(); - for (size_t i = 0; i < traj.size(); ++i) { - const float64_t dx = self_pose.position.x - traj.x[i]; - const float64_t dy = self_pose.position.y - traj.y[i]; - const float64_t dist_squared = dx * dx + dy * dy; - - /* ignore when yaw error is large, for crossing path */ - const float64_t err_yaw = autoware::common::helper_functions::wrap_angle(my_yaw - traj.yaw[i]); - if (std::fabs(err_yaw) > (M_PI / 3.0)) { - continue; - } - if (dist_squared < min_dist_squared) { - min_dist_squared = dist_squared; - nearest_idx = static_cast(i); - } - } - return nearest_idx; -} - -int64_t calcNearestIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & self_pose) -{ - if (traj.points.empty()) { - return -1; - } - const float64_t my_yaw = ::motion::motion_common::to_angle(self_pose.orientation); - int64_t nearest_idx = -1; - float64_t min_dist_squared = std::numeric_limits::max(); - for (size_t i = 0; i < traj.points.size(); ++i) { - const float64_t dx = - self_pose.position.x - static_cast(traj.points.at(i).pose.position.x); - const float64_t dy = - self_pose.position.y - static_cast(traj.points.at(i).pose.position.y); - const float64_t dist_squared = dx * dx + dy * dy; - - /* ignore when yaw error is large, for crossing path */ - const float64_t traj_yaw = - ::motion::motion_common::to_angle(traj.points.at(i).pose.orientation); - const float64_t err_yaw = autoware::common::helper_functions::wrap_angle(my_yaw - traj_yaw); - if (std::fabs(err_yaw) > (M_PI / 3.0)) { - continue; - } - if (dist_squared < min_dist_squared) { - min_dist_squared = dist_squared; - nearest_idx = static_cast(i); - } - } - return nearest_idx; -} - bool8_t calcNearestPoseInterp( const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time, - const rclcpp::Logger & logger, rclcpp::Clock & clock) + const double max_dist, const double max_yaw, const rclcpp::Logger & logger, rclcpp::Clock & clock) { if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) { return false; } - const int64_t nearest_idx = calcNearestIndex(traj, self_pose); - if (nearest_idx == -1) { + + autoware_auto_planning_msgs::msg::Trajectory autoware_traj; + convertToAutowareTrajectory(traj, autoware_traj); + if (autoware_traj.points.empty()) { RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, clock, 5000, "[calcNearestPoseInterp] fail to get nearest. traj.size = %zu", - traj.size()); + logger, clock, 5000, "[calcNearestPoseInterp] input trajectory is empty"); return false; } - const int64_t traj_size = static_cast(traj.size()); - - *nearest_index = static_cast(nearest_idx); + *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware_traj.points, self_pose, max_dist, max_yaw); + const size_t traj_size = traj.size(); if (traj.size() == 1) { nearest_pose->position.x = traj.x[*nearest_index]; @@ -390,8 +335,10 @@ bool8_t calcNearestPoseInterp( }; /* get second nearest index = next to nearest_index */ - const size_t next = static_cast(std::min(nearest_idx + 1, traj_size - 1)); - const size_t prev = static_cast(std::max(nearest_idx - 1, int64_t(0))); + const size_t next = static_cast( + std::min(static_cast(*nearest_index) + 1, static_cast(traj_size) - 1)); + const size_t prev = + static_cast(std::max(static_cast(*nearest_index) - 1, int64_t(0))); const float64_t dist_to_next = calcSquaredDist(self_pose, traj, next); const float64_t dist_to_prev = calcSquaredDist(self_pose, traj, prev); const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev; diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 1a6f3601090b6..68ec4a1656ac2 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -178,6 +178,16 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] + // ego nearest index search + m_ego_nearest_dist_threshold = + node_->has_parameter("ego_nearest_dist_threshold") + ? node_->get_parameter("ego_nearest_dist_threshold").as_double() + : node_->declare_parameter("ego_nearest_dist_threshold"); // [m] + m_ego_nearest_yaw_threshold = + node_->has_parameter("ego_nearest_yaw_threshold") + ? node_->get_parameter("ego_nearest_yaw_threshold").as_double() + : node_->declare_parameter("ego_nearest_yaw_threshold"); // [rad] + // subscriber, publisher m_pub_slope = node_->create_publisher( @@ -419,17 +429,25 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData control_data.current_motion = getCurrentMotion(); // nearest idx - const float64_t max_dist = m_state_transition_params.emergency_state_traj_trans_dev; - const float64_t max_yaw = m_state_transition_params.emergency_state_traj_rot_dev; - const auto nearest_idx_opt = - motion_common::findNearestIndex(m_trajectory_ptr->points, current_pose, max_dist, max_yaw); - - // return here if nearest index is not found - if (!nearest_idx_opt) { + const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + m_trajectory_ptr->points, current_pose, m_ego_nearest_dist_threshold, + m_ego_nearest_yaw_threshold); + const auto & nearest_point = m_trajectory_ptr->points.at(nearest_idx).pose; + + // check if the deviation is worth emergency + const bool is_dist_deviation_large = + m_state_transition_params.emergency_state_traj_trans_dev < + tier4_autoware_utils::calcDistance2d(nearest_point, current_pose); + const bool is_yaw_deviation_large = + m_state_transition_params.emergency_state_traj_rot_dev < + std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(nearest_point.orientation) - tf2::getYaw(current_pose.orientation))); + if (is_dist_deviation_large || is_yaw_deviation_large) { + // return here if nearest index is not found control_data.is_far_from_trajectory = true; return control_data; } - control_data.nearest_idx = *nearest_idx_opt; + control_data.nearest_idx = nearest_idx; // shift control_data.shift = getCurrentShift(control_data.nearest_idx); @@ -440,7 +458,7 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // distance to stopline control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( - current_pose, *m_trajectory_ptr, max_dist, max_yaw); + current_pose, *m_trajectory_ptr, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch const float64_t raw_pitch = @@ -572,7 +590,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( current_pose, m_delay_compensation_time, current_vel); const auto target_interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose, nearest_idx); + calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, target_interpolated_point.acceleration_mps2}; @@ -821,31 +839,16 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop autoware_auto_planning_msgs::msg::TrajectoryPoint PidLongitudinalController::calcInterpolatedTargetValue( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, - const size_t nearest_idx) const + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const geometry_msgs::msg::Pose & pose) const { if (traj.points.size() == 1) { return traj.points.at(0); } - // If the current position is not within the reference trajectory, enable the edge value. - // Else, apply linear interpolation - if (nearest_idx == 0) { - if (motion_common::calcSignedArcLength(traj.points, pose.position, 0) > 0) { - return traj.points.at(0); - } - } - if (nearest_idx == traj.points.size() - 1) { - if ( - motion_common::calcSignedArcLength(traj.points, pose.position, traj.points.size() - 1) < 0) { - return traj.points.at(traj.points.size() - 1); - } - } - // apply linear interpolation return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint( - traj.points, pose, m_state_transition_params.emergency_state_traj_trans_dev, - m_state_transition_params.emergency_state_traj_rot_dev); + traj.points, pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); } float64_t PidLongitudinalController::predictedVelocityInTargetPoint( @@ -941,10 +944,8 @@ void PidLongitudinalController::updateDebugVelAcc( { using trajectory_follower::DebugValues; const float64_t current_vel = control_data.current_motion.vel; - const size_t nearest_idx = control_data.nearest_idx; - const auto interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose, nearest_idx); + const auto interpolated_point = calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose); m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); diff --git a/control/trajectory_follower/test/test_mpc_utils.cpp b/control/trajectory_follower/test/test_mpc_utils.cpp index 438238d6b0011..7883537885bec 100644 --- a/control/trajectory_follower/test/test_mpc_utils.cpp +++ b/control/trajectory_follower/test/test_mpc_utils.cpp @@ -30,28 +30,6 @@ typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; typedef geometry_msgs::msg::Pose Pose; -TEST(TestMPCUtils, CalcNearestIndex) -{ - Pose pose; - pose.position.x = 0.0; - pose.position.y = 0.0; - Trajectory trajectory; - TrajectoryPoint p; - p.pose.position.x = -2.0; - p.pose.position.y = 1.0; - trajectory.points.push_back(p); - p.pose.position.x = -1.0; - p.pose.position.y = 1.0; - trajectory.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 1.0; - trajectory.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 1.0; - trajectory.points.push_back(p); - EXPECT_EQ(MPCUtils::calcNearestIndex(trajectory, pose), 2); -} - /* cppcheck-suppress syntaxError */ TEST(TestMPC, CalcStopDistance) { diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index 6ee21c45d03fc..b40bd8f16a626 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -43,7 +43,8 @@ if(BUILD_TESTING) find_package(autoware_testing REQUIRED) add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe - PARAM_FILENAMES "lateral_controller_defaults.param.yaml longitudinal_controller_defaults.param.yaml test_vehicle_info.param.yaml" + PARAM_FILENAMES "lateral_controller_defaults.param.yaml longitudinal_controller_defaults.param.yaml +test_vehicle_info.param.yaml test_nearest_search.param.yaml" ) endif() diff --git a/control/trajectory_follower_nodes/param/test_nearest_search.param.yaml b/control/trajectory_follower_nodes/param/test_nearest_search.param.yaml new file mode 100644 index 0000000000000..eb6709764ce3e --- /dev/null +++ b/control/trajectory_follower_nodes/param/test_nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/control/trajectory_follower_nodes/test/test_controller_node.cpp b/control/trajectory_follower_nodes/test/test_controller_node.cpp index ddc4fb1fe817f..d0a2b96302124 100644 --- a/control/trajectory_follower_nodes/test/test_controller_node.cpp +++ b/control/trajectory_follower_nodes/test/test_controller_node.cpp @@ -55,7 +55,8 @@ std::shared_ptr makeNode() node_options.arguments( {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", "--params-file", share_dir + "/param/longitudinal_controller_defaults.param.yaml", - "--params-file", share_dir + "/param/test_vehicle_info.param.yaml"}); + "--params-file", share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", + share_dir + "/param/test_nearest_search.param.yaml"}); std::shared_ptr node = std::make_shared(node_options); // Enable all logging in the node diff --git a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp b/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp index 38bca887d8630..0bd94c92cad92 100644 --- a/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp +++ b/control/trajectory_follower_nodes/test/test_lateral_controller_node.cpp @@ -51,7 +51,8 @@ std::shared_ptr makeLateralNode() rclcpp::NodeOptions node_options; node_options.arguments( {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", - "--params-file", share_dir + "/param/test_vehicle_info.param.yaml"}); + "--params-file", share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", + share_dir + "/param/test_nearest_search.param.yaml"}); std::shared_ptr node = std::make_shared(node_options); // Enable all logging in the node diff --git a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp index 26e00d417276f..676a9a4acfa5c 100644 --- a/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp +++ b/control/trajectory_follower_nodes/test/test_longitudinal_controller_node.cpp @@ -48,7 +48,8 @@ std::shared_ptr makeLongitudinalNode() node_options.arguments( {"--ros-args", "--params-file", share_dir + "/param/longitudinal_controller_defaults.param.yaml", "--params-file", - share_dir + "/param/test_vehicle_info.param.yaml"}); + share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", + share_dir + "/param/test_nearest_search.param.yaml"}); std::shared_ptr node = std::make_shared(node_options); diff --git a/launch/tier4_control_launch/config/common/nearest_search.param.yaml b/launch/tier4_control_launch/config/common/nearest_search.param.yaml new file mode 100644 index 0000000000000..eb6709764ce3e --- /dev/null +++ b/launch/tier4_control_launch/config/common/nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index e1dbc769deab8..106d1e8ef0d1b 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -44,6 +44,14 @@ def launch_setup(context, *args, **kwargs): with open(lat_controller_param_path, "r") as f: lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + nearest_search_param_path = os.path.join( + LaunchConfiguration("tier4_control_launch_param_path").perform(context), + "common", + "nearest_search.param.yaml", + ) + with open(nearest_search_param_path, "r") as f: + nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] + lon_controller_param_path = os.path.join( LaunchConfiguration("tier4_control_launch_param_path").perform(context), "trajectory_follower", @@ -94,6 +102,7 @@ def launch_setup(context, *args, **kwargs): "ctrl_period": 0.03, "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"), }, + nearest_search_param, lon_controller_param, lat_controller_param, vehicle_info_param, @@ -117,7 +126,7 @@ def launch_setup(context, *args, **kwargs): "/control/trajectory_follower/lateral/predicted_trajectory", ), ], - parameters=[lane_departure_checker_param, vehicle_info_param], + parameters=[nearest_search_param, lane_departure_checker_param, vehicle_info_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -203,6 +212,7 @@ def launch_setup(context, *args, **kwargs): ("control_mode_request", "/control/control_mode_request"), ], parameters=[ + nearest_search_param_path, operation_mode_transition_manager_param, vehicle_info_param, ],