From 47ac4433dace9b13dbeb5ee39b786e1a98ae546b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 15 Aug 2022 19:00:26 +0900 Subject: [PATCH 1/7] fix(trajectory_follower): use common ego nearest search 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 | 31 ++-- .../pid_longitudinal_controller.hpp | 6 +- .../src/longitudinal_controller_utils.cpp | 26 +--- control/trajectory_follower/src/mpc.cpp | 17 ++- .../src/mpc_lateral_controller.cpp | 12 +- control/trajectory_follower/src/mpc_utils.cpp | 137 +++++++++--------- .../src/pid_longitudinal_controller.cpp | 57 ++++---- .../test/test_mpc_utils.cpp | 42 +++--- 11 files changed, 177 insertions(+), 170 deletions(-) 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..b93b1f0c97aef 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 ego_nearest_dist_threshold_; + double 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..a3ca41f0b0e24 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -176,27 +176,28 @@ 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); + const double max_dist, const double max_yaw, 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 - */ +// 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 +// */ 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..8a091069842b8 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 ego_nearest_dist_threshold_; + double 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..0507a69575293 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -69,28 +69,16 @@ 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); + 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, end_idx); - if (std::isnan(signed_length_src_offset)) { + 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..726fd02814393 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,7 +490,11 @@ 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); + 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); if (nearest_idx < 0) { return input; } @@ -783,8 +787,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..4c63f649ff41c 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,12 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); } + // ego nearest index search + ego_nearest_dist_threshold_ = node_->declare_parameter("ego_nearest_dist_threshold"); + ego_nearest_yaw_threshold_ = node_->declare_parameter("ego_nearest_yaw_threshold"); + m_mpc.ego_nearest_dist_threshold = ego_nearest_dist_threshold_; + m_mpc.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold_; + m_pub_predicted_traj = node_->create_publisher( "~/output/predicted_trajectory", 1); m_pub_diagnostic = @@ -360,8 +367,9 @@ bool8_t MpcLateralController::isStoppedState() const // 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); + const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints( + m_current_trajectory_ptr->points, m_current_pose_ptr->pose, ego_nearest_dist_threshold_, + ego_nearest_yaw_threshold_); // If the nearest index is not found, return false if (nearest < 0) { diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index 9f91c8feedc31..1eb9a7bf4d245 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,84 +298,79 @@ 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; -} +// 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) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, clock, 5000, "[calcNearestPoseInterp] fail to get nearest. traj.size = %zu", - traj.size()); - return false; - } + autoware_auto_planning_msgs::msg::Trajectory autoware_traj; + convertToAutowareTrajectory(traj, autoware_traj); + *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints( + autoware_traj.points, self_pose, max_dist, max_yaw); const int64_t traj_size = static_cast(traj.size()); - *nearest_index = static_cast(nearest_idx); - if (traj.size() == 1) { nearest_pose->position.x = traj.x[*nearest_index]; nearest_pose->position.y = traj.y[*nearest_index]; @@ -390,8 +387,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, 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..85a3351537e2c 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -178,6 +178,12 @@ 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 + ego_nearest_dist_threshold_ = + node_->declare_parameter("ego_nearest_dist_threshold"); // [m] + ego_nearest_yaw_threshold_ = + node_->declare_parameter("ego_nearest_yaw_threshold"); // [rad] + // subscriber, publisher m_pub_slope = node_->create_publisher( @@ -419,17 +425,23 @@ 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, ego_nearest_dist_threshold_, + 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(tf2::getYaw(nearest_point.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 +452,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, ego_nearest_dist_threshold_, ego_nearest_yaw_threshold_); // pitch const float64_t raw_pitch = @@ -572,7 +584,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 +833,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, ego_nearest_dist_threshold_, ego_nearest_yaw_threshold_); } float64_t PidLongitudinalController::predictedVelocityInTargetPoint( @@ -941,10 +938,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..17799c61e9c96 100644 --- a/control/trajectory_follower/test/test_mpc_utils.cpp +++ b/control/trajectory_follower/test/test_mpc_utils.cpp @@ -30,27 +30,27 @@ 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); -} +// 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) From 9be6d98a73f08667765d01f684e7064d59c9fb0a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 17 Aug 2022 11:46:37 +0900 Subject: [PATCH 2/7] removed calcNearestIndex Signed-off-by: Takayuki Murooka --- .../include/trajectory_follower/mpc_utils.hpp | 17 ----- .../src/mpc_lateral_controller.cpp | 10 +-- control/trajectory_follower/src/mpc_utils.cpp | 66 +++---------------- .../test/test_mpc_utils.cpp | 22 ------- 4 files changed, 13 insertions(+), 102 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp index a3ca41f0b0e24..83f152f3e4df1 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -178,23 +178,6 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcNearestPoseInterp( geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time, const double max_dist, const double max_yaw, 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 // */ diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index 4c63f649ff41c..4a0509cdfbdba 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -363,6 +363,11 @@ 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 @@ -371,11 +376,6 @@ bool8_t MpcLateralController::isStoppedState() const m_current_trajectory_ptr->points, m_current_pose_ptr->pose, ego_nearest_dist_threshold_, ego_nearest_yaw_threshold_); - // If the nearest index is not found, return false - if (nearest < 0) { - return false; - } - const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x; const float64_t target_vel = m_current_trajectory_ptr->points.at(static_cast(nearest)).longitudinal_velocity_mps; diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index 1eb9a7bf4d245..e057e52e10938 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -298,64 +298,6 @@ 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, @@ -367,6 +309,14 @@ bool8_t calcNearestPoseInterp( autoware_auto_planning_msgs::msg::Trajectory autoware_traj; convertToAutowareTrajectory(traj, autoware_traj); + if (autoware_traj.points.size().empty()) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, clock, 5000, + "[calcNearestPoseInterp] fail to get nearest. autoware_traj.points.size = %zu", + autoware_traj.points.size()); + return false; + } + *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, self_pose, max_dist, max_yaw); const int64_t traj_size = static_cast(traj.size()); diff --git a/control/trajectory_follower/test/test_mpc_utils.cpp b/control/trajectory_follower/test/test_mpc_utils.cpp index 17799c61e9c96..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) { From 1a635b74999d3313df8fa6d050ca5e2ac5318f4d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 15 Aug 2022 19:14:56 +0900 Subject: [PATCH 3/7] add nearest search param Signed-off-by: Takayuki Murooka --- .../config/common/nearest_search.param.yaml | 5 +++++ launch/tier4_control_launch/launch/control.launch.py | 12 +++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) create mode 100644 launch/tier4_control_launch/config/common/nearest_search.param.yaml 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..150482dbb7505 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_path, 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, ], From b692da283041cfb9a73d4e158475a173cb021f35 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 17 Aug 2022 12:38:35 +0900 Subject: [PATCH 4/7] fix Signed-off-by: Takayuki Murooka --- .../src/longitudinal_controller_utils.cpp | 5 +++-- .../src/mpc_lateral_controller.cpp | 10 ++++++++-- control/trajectory_follower/src/mpc_utils.cpp | 2 +- .../src/pid_longitudinal_controller.cpp | 14 ++++++++++---- 4 files changed, 22 insertions(+), 9 deletions(-) diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 0507a69575293..55060d1b9bc7f 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -72,8 +72,9 @@ float64_t calcStopDistance( 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, end_idx); + 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; diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index 4a0509cdfbdba..d1ee70fb3780d 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -142,8 +142,14 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} } // ego nearest index search - ego_nearest_dist_threshold_ = node_->declare_parameter("ego_nearest_dist_threshold"); - ego_nearest_yaw_threshold_ = node_->declare_parameter("ego_nearest_yaw_threshold"); + 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] + 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 = ego_nearest_dist_threshold_; m_mpc.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold_; diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index e057e52e10938..75df309e5ff4d 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -309,7 +309,7 @@ bool8_t calcNearestPoseInterp( autoware_auto_planning_msgs::msg::Trajectory autoware_traj; convertToAutowareTrajectory(traj, autoware_traj); - if (autoware_traj.points.size().empty()) { + if (autoware_traj.points.empty()) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger, clock, 5000, "[calcNearestPoseInterp] fail to get nearest. autoware_traj.points.size = %zu", diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 85a3351537e2c..d888c94ae6b18 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -180,9 +180,13 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node // ego nearest index search ego_nearest_dist_threshold_ = - node_->declare_parameter("ego_nearest_dist_threshold"); // [m] + node_->has_parameter("ego_nearest_dist_threshold") + ? node_->get_parameter("ego_nearest_dist_threshold").as_double() + : node_->declare_parameter("ego_nearest_dist_threshold"); // [m] ego_nearest_yaw_threshold_ = - node_->declare_parameter("ego_nearest_yaw_threshold"); // [rad] + 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 = @@ -434,8 +438,10 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData 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(tf2::getYaw(nearest_point.orientation)); + 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; From 1bf19103d8ae2a45315b23c0e5a54d78e810edb6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 17 Aug 2022 23:20:16 +0900 Subject: [PATCH 5/7] fix Signed-off-by: Takayuki Murooka --- .../trajectory_follower/mpc_lateral_controller.hpp | 4 ++-- .../pid_longitudinal_controller.hpp | 4 ++-- control/trajectory_follower/src/mpc.cpp | 7 ++++--- .../src/mpc_lateral_controller.cpp | 12 ++++++------ control/trajectory_follower/src/mpc_utils.cpp | 10 ++++------ .../src/pid_longitudinal_controller.cpp | 12 ++++++------ 6 files changed, 24 insertions(+), 25 deletions(-) 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 b93b1f0c97aef..bfbf0d4834c13 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -139,8 +139,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; // ego nearest index search - double ego_nearest_dist_threshold_; - double ego_nearest_yaw_threshold_; + 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/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 8a091069842b8..31efe59c64b18 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -187,8 +187,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal float64_t m_min_pitch_rad; // ego nearest index search - double ego_nearest_dist_threshold_; - double ego_nearest_yaw_threshold_; + 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}; diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index 726fd02814393..a281a226a58cb 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -493,12 +493,13 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( 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); - if (nearest_idx < 0) { + 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; diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index d1ee70fb3780d..8dd198bccb8ed 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -142,16 +142,16 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} } // ego nearest index search - ego_nearest_dist_threshold_ = + 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] - ego_nearest_yaw_threshold_ = + 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 = ego_nearest_dist_threshold_; - m_mpc.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold_; + 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); @@ -379,8 +379,8 @@ bool8_t MpcLateralController::isStoppedState() const // control was turned off when approaching/exceeding the stop line on a curve or // emergency stop situation and it caused large tracking error. const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_current_trajectory_ptr->points, m_current_pose_ptr->pose, ego_nearest_dist_threshold_, - ego_nearest_yaw_threshold_); + 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 75df309e5ff4d..a56dc36a007de 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -311,15 +311,13 @@ bool8_t calcNearestPoseInterp( convertToAutowareTrajectory(traj, autoware_traj); if (autoware_traj.points.empty()) { RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, clock, 5000, - "[calcNearestPoseInterp] fail to get nearest. autoware_traj.points.size = %zu", - autoware_traj.points.size()); + logger, clock, 5000, "[calcNearestPoseInterp] input trajectory is empty"); return false; } *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints( autoware_traj.points, self_pose, max_dist, max_yaw); - const int64_t traj_size = static_cast(traj.size()); + const size_t traj_size = traj.size(); if (traj.size() == 1) { nearest_pose->position.x = traj.x[*nearest_index]; @@ -337,8 +335,8 @@ bool8_t calcNearestPoseInterp( }; /* get second nearest index = next to nearest_index */ - const size_t next = - static_cast(std::min(static_cast(*nearest_index) + 1, traj_size - 1)); + 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); diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index d888c94ae6b18..68ec4a1656ac2 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -179,11 +179,11 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node m_min_pitch_rad = node_->declare_parameter("min_pitch_rad"); // [rad] // ego nearest index search - ego_nearest_dist_threshold_ = + 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] - ego_nearest_yaw_threshold_ = + 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] @@ -430,8 +430,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // nearest idx const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_trajectory_ptr->points, current_pose, ego_nearest_dist_threshold_, - ego_nearest_yaw_threshold_); + 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 @@ -458,7 +458,7 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // distance to stopline control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( - current_pose, *m_trajectory_ptr, ego_nearest_dist_threshold_, ego_nearest_yaw_threshold_); + current_pose, *m_trajectory_ptr, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch const float64_t raw_pitch = @@ -848,7 +848,7 @@ PidLongitudinalController::calcInterpolatedTargetValue( // apply linear interpolation return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint( - traj.points, pose, ego_nearest_dist_threshold_, ego_nearest_yaw_threshold_); + traj.points, pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); } float64_t PidLongitudinalController::predictedVelocityInTargetPoint( From f035c0b31c21f4ece19f208f7ab014cfdee7947b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 Aug 2022 10:13:49 +0900 Subject: [PATCH 6/7] Update launch/tier4_control_launch/launch/control.launch.py --- launch/tier4_control_launch/launch/control.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 150482dbb7505..106d1e8ef0d1b 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -126,7 +126,7 @@ def launch_setup(context, *args, **kwargs): "/control/trajectory_follower/lateral/predicted_trajectory", ), ], - parameters=[nearest_search_param_path, 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")}], ) From fd1c06db8484c7c1b638faa31feb2173b15dd21b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 Aug 2022 11:44:11 +0900 Subject: [PATCH 7/7] update test fil Signed-off-by: Takayuki Murooka --- control/trajectory_follower_nodes/CMakeLists.txt | 3 ++- .../param/test_nearest_search.param.yaml | 5 +++++ .../trajectory_follower_nodes/test/test_controller_node.cpp | 3 ++- .../test/test_lateral_controller_node.cpp | 3 ++- .../test/test_longitudinal_controller_node.cpp | 3 ++- 5 files changed, 13 insertions(+), 4 deletions(-) create mode 100644 control/trajectory_follower_nodes/param/test_nearest_search.param.yaml 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);