Skip to content

Commit

Permalink
problemler fixed
Browse files Browse the repository at this point in the history 
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
  • Loading branch information
@brkay54
brkay54 committed Aug 22, 2023
1 parent cdfb2e9 commit 32c45f9
Show file tree
Hide file tree
Showing 5 changed files with 202 additions and 181 deletions.
9 changes: 3 additions & 6 deletions ...itudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@
#include <cmath>
#include <limits>


namespace autoware::motion::control::pid_longitudinal_controller::longitudinal_utils
{

Expand Down Expand Up @@ -74,9 +73,8 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint
* @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and
* acceleration for delayed time
*/
Pose calcPoseAfterTimeDelay(
const Pose & current_pose, const double delay_time, const double current_vel,
const double current_acc);
std::pair<double, double> calcDistAndVelAfterTimeDelay(
const double delay_time, const double current_vel, const double current_acc);

/**
* @brief apply linear interpolation to orientation
Expand Down Expand Up @@ -160,7 +158,6 @@ double applyDiffLimitFilter(
geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
const size_t src_idx, const double distance,
const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
} // namespace autoware::motion::control::pid_longitudinal_controller::longitudinal_utils

} // namespace autoware::motion::control::pid_longitudinal_controller::longitudinal_utils

#endif // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_
7 changes: 2 additions & 5 deletions ...ngitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -367,13 +367,10 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro

/**
* @brief calculate velocity feedback with feed forward and pid controller
* @param [in] target_motion reference velocity and acceleration. This acceleration will be used
* as feed forward.
* @param [in] dt time step to use
* @param [in] current_vel current velocity of the vehicle
* @param [in] control_data data for control calculation
*/
double applyVelocityFeedback(
const Motion target_motion, const double dt, const double current_vel);
const ControlData & control_data);

/**
* @brief update variables for debugging about pitch
Expand Down
23 changes: 10 additions & 13 deletions control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,12 +122,11 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint
return pitch;
}

Pose calcPoseAfterTimeDelay(
const Pose & current_pose, const double delay_time, const double current_vel,
const double current_acc)
std::pair<double, double> calcDistAndVelAfterTimeDelay(
const double delay_time, const double current_vel, const double current_acc)
{
if (delay_time <= 0.0) {
return current_pose;
return std::make_pair(0.0, 0.0);
}

// check time to stop
Expand All @@ -136,17 +135,11 @@ Pose calcPoseAfterTimeDelay(
const double delay_time_calculation =
time_to_stop > 0.0 && time_to_stop < delay_time ? time_to_stop : delay_time;
// simple linear prediction
const double yaw = tf2::getYaw(current_pose.orientation);
const double vel_after_delay = current_vel + current_acc * delay_time_calculation;
const double running_distance = delay_time_calculation * current_vel + 0.5 * current_acc *
delay_time_calculation *
delay_time_calculation;
const double dx = running_distance * std::cos(yaw);
const double dy = running_distance * std::sin(yaw);

auto pred_pose = current_pose;
pred_pose.position.x += dx;
pred_pose.position.y += dy;
return pred_pose;
return std::make_pair(running_distance, vel_after_delay);
}

double lerp(const double v_from, const double v_to, const double ratio)
Expand Down Expand Up @@ -190,9 +183,12 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
double remain_dist = distance;
geometry_msgs::msg::Pose p = trajectory.points.back().pose;
for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) {
double dist = tier4_autoware_utils::calcDistance3d(
const double dist = tier4_autoware_utils::calcDistance3d(
trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose);
if (remain_dist < dist) {
if(remain_dist <= 0.0){
return trajectory.points.at(i).pose;
}
double ratio = remain_dist / dist;
p = trajectory.points.at(i).pose;
p.position.x = trajectory.points.at(i).pose.position.x +
Expand All @@ -204,6 +200,7 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
p.position.z = trajectory.points.at(i).pose.position.z +
ratio * (trajectory.points.at(i + 1).pose.position.z -
trajectory.points.at(i).pose.position.z);
break;
}
remain_dist -= dist;
}
Expand Down
86 changes: 58 additions & 28 deletions control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -449,20 +449,27 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
control_data.state_after_delay =
predictedStateAfterDelay(control_data.current_motion, m_delay_compensation_time);

const auto target_pose = longitudinal_utils::findTrajectoryPoseAfterDistance(
nearest_idx, control_data.state_after_delay.running_distance, control_data.interpolated_traj);
const auto target_interpolated_point =
calcInterpolatedTrajPointAndSegment(m_trajectory, target_pose);

// insert the interpolated point
const auto interpolated_idx = target_interpolated_point.second + 1;
control_data.interpolated_traj = m_trajectory;
control_data.interpolated_traj.points.insert(
control_data.interpolated_traj.points.begin() + interpolated_idx,
target_interpolated_point.first);
// calculate the target motion
control_data.target_idx = control_data.nearest_idx;
if (control_data.state_after_delay.running_distance > 0.01) {
const auto target_pose = longitudinal_utils::findTrajectoryPoseAfterDistance(
control_data.nearest_idx, control_data.state_after_delay.running_distance,
control_data.interpolated_traj);
const auto target_interpolated_point =
calcInterpolatedTrajPointAndSegment(control_data.interpolated_traj, target_pose);
control_data.target_idx = target_interpolated_point.second + 1;
control_data.interpolated_traj.points.insert(
control_data.interpolated_traj.points.begin() + control_data.target_idx,
target_interpolated_point.first);
}

std::cout << control_data.state_after_delay.running_distance << " " << control_data.nearest_idx
<< " " << control_data.target_idx << std::endl;
// calculate the predicted velocity in the target point
m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, control_data.state_after_delay.vel);
m_debug_values.setValues(
DebugValues::TYPE::TARGET_VEL,
control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps);

// shift
control_data.shift = getCurrentShift(control_data);
Expand All @@ -474,8 +481,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
// distance to stopline

control_data.stop_dist = longitudinal_utils::calcStopDistance(
target_pose, control_data.interpolated_traj, m_ego_nearest_dist_threshold,
m_ego_nearest_yaw_threshold);
control_data.interpolated_traj.points.at(control_data.target_idx).pose,
control_data.interpolated_traj, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold);

// pitch
constexpr double stopeed_vel = 0.01;
Expand Down Expand Up @@ -646,15 +653,14 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(

// velocity and acceleration command
Motion raw_ctrl_cmd{};
Motion target_motion{control_data.state_after_delay.vel, control_data.state_after_delay.acc};
Motion target_motion{
control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps,
control_data.interpolated_traj.points.at(target_idx).acceleration_mps2};
if (m_control_state == ControlState::DRIVE) {
target_motion = keepBrakeBeforeStop(m_trajectory, target_motion, target_idx);

m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, control_data.state_after_delay.vel);

raw_ctrl_cmd.vel = target_motion.vel;
raw_ctrl_cmd.acc =
applyVelocityFeedback(target_motion, control_data.dt, control_data.state_after_delay.vel);
raw_ctrl_cmd.acc = applyVelocityFeedback(control_data);
RCLCPP_DEBUG(
node_->get_logger(),
"[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f "
Expand Down Expand Up @@ -937,15 +943,16 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS
return StateAfterDelay{pred_vel, pred_acc, running_distance};
}

double PidLongitudinalController::applyVelocityFeedback(
const Motion target_motion, const double dt, const double current_vel)
double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data)
{
const double current_vel_abs = std::fabs(current_vel);
const double target_vel_abs = std::fabs(target_motion.vel);
const double vel_after_delay_abs = std::fabs(control_data.state_after_delay.vel);
const double target_vel_abs = std::fabs(
control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps);
const double dt = control_data.dt;
const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
const bool enable_integration =
(current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs);
(vel_after_delay_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - vel_after_delay_abs);

std::vector<double> pid_contributions(3);
const double pid_acc =
Expand All @@ -959,8 +966,27 @@ double PidLongitudinalController::applyVelocityFeedback(
constexpr double ff_scale_max = 2.0; // for safety
constexpr double ff_scale_min = 0.5; // for safety
const double ff_scale =
std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max);
const double ff_acc = target_motion.acc * ff_scale;
std::clamp(vel_after_delay_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max);

// calculate the position of a target point which is delay time after our target point
// for FF calculation
// const auto look_forward_dist_and_vel = longitudinal_utils::calcDistAndVelAfterTimeDelay(
// m_delay_compensation_time, control_data.state_after_delay.vel,
// control_data.state_after_delay.acc);

// calculate the FF acceleration
// const double ff_acc_tmp =
// look_forward_dist_and_vel.first < 0.01
// ? control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2
// : 0.5 * (std::pow(look_forward_dist_and_vel.second, 2) - std::pow(vel_after_delay_abs, 2))
// /
// look_forward_dist_and_vel.first;
// std::cout << "look_forward_dist_and_vel.second: " << look_forward_dist_and_vel.second
// << " vel_after_delay: " << vel_after_delay_abs << " ff acc: " << ff_acc_tmp
// << std::endl;
// Feedforward acceleration:
const double ff_acc =
control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale;

const double feedback_acc = ff_acc + pid_acc;

Expand Down Expand Up @@ -993,8 +1019,12 @@ void PidLongitudinalController::updateDebugVelAcc(
const double current_vel = control_data.current_motion.vel;

m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel);
m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel);
m_debug_values.setValues(DebugValues::TYPE::TARGET_ACC, target_motion.acc);
m_debug_values.setValues(
DebugValues::TYPE::TARGET_VEL,
control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps);
m_debug_values.setValues(
DebugValues::TYPE::TARGET_ACC,
control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2);
m_debug_values.setValues(
DebugValues::TYPE::NEAREST_VEL,
control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
Expand Down
Loading

0 comments on commit 32c45f9

Please sign in to comment.