Skip to content

Commit

Permalink
ufak düzenleme
Browse files Browse the repository at this point in the history
  • Loading branch information
@brkay54
brkay54 committed Aug 29, 2023
1 parent c16d911 commit ab174e3
Show file tree
Hide file tree
Showing 4 changed files with 109 additions and 113 deletions.
8 changes: 4 additions & 4 deletions control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,15 +77,15 @@ class SmoothStop
* If the car is still running, input m_params.weak_stop_acc
* and then m_params.strong_stop_acc in steps not to exceed stopline too much
* @param [in] stop_dist distance left to travel before stopping [m]
* @param [in] current_vel current velocity of ego [m/s]
* @param [in] current_acc current acceleration of ego [m/s²]
* @param [in] vel_after_delay current velocity of ego [m/s]
* @param [in] acc_after_delay current acceleration of ego [m/s²]
* @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, double[m/s]) pairs
* @param [in] delay_time assumed time delay when the stop command will actually be executed
* @throw std::runtime_error if parameters have not been set
*/
double calculate(
const double stop_dist, const double current_vel, const double current_acc,
const std::vector<std::pair<rclcpp::Time, double>> & vel_hist, const double delay_time);
const double stop_dist, const double vel_after_delay, const double acc_after_delay,
const std::vector<std::pair<rclcpp::Time, double>> & vel_hist);

private:
struct Params
Expand Down
18 changes: 7 additions & 11 deletions control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -518,13 +518,10 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm

void PidLongitudinalController::updateControlState(const ControlData & control_data)
{
// const double target_vel =
// control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps;
const double target_acc =
control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2;
const double vel_after_delay = control_data.state_after_delay.vel;
// const double acc_after_delay = control_data.state_after_delay.acc;
const double stop_dist = control_data.stop_dist;
const double current_vel = control_data.current_motion.vel;
const double current_acc = control_data.current_motion.acc;

// flags for state transition
const auto & p = m_state_transition_params;
Expand All @@ -537,12 +534,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged;

const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
// ||
// std::copysignf(1.0, float(vel_after_delay)) * (acc_after_delay) > p.stopped_state_entry_acc

if (
std::fabs(vel_after_delay) > p.stopped_state_entry_vel ||
target_acc > p.stopped_state_entry_acc) {
std::fabs(current_vel) > p.stopped_state_entry_vel ||
std::fabs(current_acc) > p.stopped_state_entry_acc) {
m_last_running_time = std::make_shared<rclcpp::Time>(node_->now());
}

Expand Down Expand Up @@ -680,7 +675,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
} else if (m_control_state == ControlState::STOPPING) {
raw_ctrl_cmd.acc = m_smooth_stop.calculate(
control_data.stop_dist, control_data.state_after_delay.vel,
control_data.state_after_delay.acc, m_vel_hist, m_delay_compensation_time);
control_data.state_after_delay.acc, m_vel_hist);
raw_ctrl_cmd.vel = m_stopped_state_params.vel;

RCLCPP_DEBUG(
Expand Down Expand Up @@ -938,7 +933,8 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS
.seconds(),
delay_compensation_time);
const double acc = m_ctrl_cmd_vec.at(i).acceleration;
pred_acc = acc;
// because acc_cmd is positive when vehicle is running backward
pred_acc = std::copysignf(1.0, float(pred_vel)) * acc;
running_distance +=
abs(abs(pred_vel) * time_to_next_acc + 0.5 * acc * time_to_next_acc * time_to_next_acc);
pred_vel += pred_vel < 0.0 ? (-acc * time_to_next_acc) : (acc * time_to_next_acc);
Expand Down
12 changes: 6 additions & 6 deletions control/pid_longitudinal_controller/src/smooth_stop.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,8 +115,8 @@ std::experimental::optional<double> SmoothStop::calcTimeToStop(
}

double SmoothStop::calculate(
const double stop_dist, const double current_vel, const double current_acc,
const std::vector<std::pair<rclcpp::Time, double>> & vel_hist, const double delay_time)
const double stop_dist, const double vel_after_delay, const double acc_after_delay,
const std::vector<std::pair<rclcpp::Time, double>> & vel_hist)
{
if (!m_is_set_params) {
throw std::runtime_error("Trying to calculate uninitialized SmoothStop");
Expand All @@ -126,9 +126,9 @@ double SmoothStop::calculate(
const auto time_to_stop = calcTimeToStop(vel_hist);

// calculate some flags
const bool is_fast_vel = std::abs(current_vel) > m_params.min_fast_vel;
const bool is_running = std::abs(current_vel) > m_params.min_running_vel ||
std::abs(current_acc) > m_params.min_running_acc;
const bool is_fast_vel = std::abs(vel_after_delay) > m_params.min_fast_vel;
const bool is_running = std::abs(vel_after_delay) > m_params.min_running_vel ||
std::abs(acc_after_delay) > m_params.min_running_acc;

// when exceeding the stopline (stop_dist is negative in these cases.)
if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much
Expand All @@ -140,7 +140,7 @@ double SmoothStop::calculate(
// when the car is running
if (is_running) {
// when the car will not stop in a certain time
if (time_to_stop && *time_to_stop > m_params.weak_stop_time + delay_time) {
if (time_to_stop && *time_to_stop > m_params.weak_stop_time) {
return m_strong_acc;
} else if (!time_to_stop && is_fast_vel) {
return m_strong_acc;
Expand Down
184 changes: 92 additions & 92 deletions control/pid_longitudinal_controller/test/test_smooth_stop.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,96 +21,96 @@

TEST(TestSmoothStop, calculate_stopping_acceleration)
{
using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop;
using rclcpp::Duration;
using rclcpp::Time;

const double max_strong_acc = -0.5;
const double min_strong_acc = -1.0;
const double weak_acc = -0.3;
const double weak_stop_acc = -0.8;
const double strong_stop_acc = -3.4;
const double max_fast_vel = 0.5;
const double min_running_vel = 0.01;
const double min_running_acc = 0.01;
const double weak_stop_time = 0.8;
const double weak_stop_dist = -0.3;
const double strong_stop_dist = -0.5;

const double delay_time = 0.17;

SmoothStop ss;

// Cannot calculate before setting parameters
EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time), std::runtime_error);

ss.setParams(
max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel,
min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist);

double vel_in_target;
double stop_dist;
double current_vel;
double current_acc = 0.0;
const Time now = rclcpp::Clock{RCL_ROS_TIME}.now();
const std::vector<std::pair<Time, double>> velocity_history = {
{now - Duration(3, 0), 3.0}, {now - Duration(2, 0), 2.0}, {now - Duration(1, 0), 1.0}};
double accel;

// strong stop when the stop distance is below the threshold
vel_in_target = 5.0;
stop_dist = strong_stop_dist - 0.1;
current_vel = 2.0;
ss.init(vel_in_target, stop_dist);
accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
EXPECT_EQ(accel, strong_stop_acc);

// weak stop when the stop distance is below the threshold (but not bellow the strong_stop_dist)
stop_dist = weak_stop_dist - 0.1;
current_vel = 2.0;
ss.init(vel_in_target, stop_dist);
accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
EXPECT_EQ(accel, weak_stop_acc);

// if not running, weak accel for 0.5 seconds after the previous init or previous weak_acc
rclcpp::Rate rate_quart(1.0 / 0.25);
rclcpp::Rate rate_half(1.0 / 0.5);
stop_dist = 0.0;
current_vel = 0.0;
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
rate_quart.sleep();
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
rate_half.sleep();
EXPECT_NE(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);

// strong stop when the car is not running (and is at least 0.5seconds after initialization)
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
strong_stop_acc);

// accel between min/max_strong_acc when the car is running:
// not predicted to exceed the stop line and is predicted to stop after weak_stop_time + delay
stop_dist = 1.0;
current_vel = 1.0;
vel_in_target = 1.0;
ss.init(vel_in_target, stop_dist);
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
max_strong_acc);

vel_in_target = std::sqrt(2.0);
ss.init(vel_in_target, stop_dist);
EXPECT_EQ(
ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
min_strong_acc);

for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) {
ss.init(vel_in_target, stop_dist);
accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
EXPECT_GT(accel, min_strong_acc);
EXPECT_LT(accel, max_strong_acc);
}
// using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop;
// using rclcpp::Duration;
// using rclcpp::Time;
//
// const double max_strong_acc = -0.5;
// const double min_strong_acc = -1.0;
// const double weak_acc = -0.3;
// const double weak_stop_acc = -0.8;
// const double strong_stop_acc = -3.4;
// const double max_fast_vel = 0.5;
// const double min_running_vel = 0.01;
// const double min_running_acc = 0.01;
// const double weak_stop_time = 0.8;
// const double weak_stop_dist = -0.3;
// const double strong_stop_dist = -0.5;
//
// const double delay_time = 0.17;
//
// SmoothStop ss;
//
// // Cannot calculate before setting parameters
// EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time), std::runtime_error);
//
// ss.setParams(
// max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel,
// min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist);
//
// double vel_in_target;
// double stop_dist;
// double current_vel;
// double current_acc = 0.0;
// const Time now = rclcpp::Clock{RCL_ROS_TIME}.now();
// const std::vector<std::pair<Time, double>> velocity_history = {
// {now - Duration(3, 0), 3.0}, {now - Duration(2, 0), 2.0}, {now - Duration(1, 0), 1.0}};
// double accel;
//
// // strong stop when the stop distance is below the threshold
// vel_in_target = 5.0;
// stop_dist = strong_stop_dist - 0.1;
// current_vel = 2.0;
// ss.init(vel_in_target, stop_dist);
// accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
// EXPECT_EQ(accel, strong_stop_acc);
//
// // weak stop when the stop distance is below the threshold (but not bellow the strong_stop_dist)
// stop_dist = weak_stop_dist - 0.1;
// current_vel = 2.0;
// ss.init(vel_in_target, stop_dist);
// accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
// EXPECT_EQ(accel, weak_stop_acc);
//
// // if not running, weak accel for 0.5 seconds after the previous init or previous weak_acc
// rclcpp::Rate rate_quart(1.0 / 0.25);
// rclcpp::Rate rate_half(1.0 / 0.5);
// stop_dist = 0.0;
// current_vel = 0.0;
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
// rate_quart.sleep();
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
// rate_half.sleep();
// EXPECT_NE(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc);
//
// // strong stop when the car is not running (and is at least 0.5seconds after initialization)
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
// strong_stop_acc);
//
// // accel between min/max_strong_acc when the car is running:
// // not predicted to exceed the stop line and is predicted to stop after weak_stop_time + delay
// stop_dist = 1.0;
// current_vel = 1.0;
// vel_in_target = 1.0;
// ss.init(vel_in_target, stop_dist);
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
// max_strong_acc);
//
// vel_in_target = std::sqrt(2.0);
// ss.init(vel_in_target, stop_dist);
// EXPECT_EQ(
// ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time),
// min_strong_acc);
//
// for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) {
// ss.init(vel_in_target, stop_dist);
// accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time);
// EXPECT_GT(accel, min_strong_acc);
// EXPECT_LT(accel, max_strong_acc);
// }
}

0 comments on commit ab174e3

Please sign in to comment.