Skip to content

Commit

Permalink
feat(pid_longitudinal_controller): fix some bugs and improve delay co…
Browse files Browse the repository at this point in the history 
…mpensation autowarefoundation#4712
  • Loading branch information
@LeoDriveProject
LeoDriveProject committed Sep 14, 2023
1 parent 8798d5c commit fcc9756
Show file tree
Hide file tree
Showing 6 changed files with 632 additions and 499 deletions.
25 changes: 19 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 @@ -75,9 +75,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 All @@ -93,7 +92,7 @@ Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, c
* @param [in] point Interpolated point is nearest to this point.
*/
template <class T>
TrajectoryPoint lerpTrajectoryPoint(
std::pair<TrajectoryPoint, size_t> lerpTrajectoryPoint(
const T & points, const Pose & pose, const double max_dist, const double max_yaw)
{
TrajectoryPoint interpolated_point;
Expand All @@ -112,6 +111,8 @@ TrajectoryPoint lerpTrajectoryPoint(
points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
interpolated_point.pose.position.y = interpolation::lerp(
points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
interpolated_point.pose.position.z = interpolation::lerp(
points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio);
interpolated_point.pose.orientation = lerpOrientation(
points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
Expand All @@ -125,7 +126,7 @@ TrajectoryPoint lerpTrajectoryPoint(
points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio);
}

return interpolated_point;
return std::make_pair(interpolated_point, seg_idx);
}

/**
Expand All @@ -149,7 +150,19 @@ double applyDiffLimitFilter(
double applyDiffLimitFilter(
const double input_val, const double prev_val, const double dt, const double max_val,
const double min_val);

/**
* @brief calculate the projected pose after distance from the current index
* @param [in] src_idx current index
* @param [in] distance distance to project
* @param [in] trajectory reference trajectory
*/
geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
const size_t src_idx, const double distance,
const autoware_auto_planning_msgs::msg::Trajectory & trajectory);

TrajectoryPoint getExtendTrajectoryPoint(
const double extend_distance, const TrajectoryPoint & goal_point);
} // namespace longitudinal_utils
} // namespace autoware::motion::control::pid_longitudinal_controller

#endif // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_
43 changes: 23 additions & 20 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 @@ -65,13 +65,25 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
double vel{0.0};
double acc{0.0};
};

struct StateAfterDelay
{
StateAfterDelay(const double velocity, const double acceleration, const double distance)
: vel(velocity), acc(acceleration), running_distance(distance)
{
}
double vel{0.0};
double acc{0.0};
double running_distance{0.0};
};
enum class Shift { Forward = 0, Reverse };

struct ControlData
{
bool is_far_from_trajectory{false};
autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{};
size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx
size_t target_idx{0};
StateAfterDelay state_after_delay{0.0, 0.0, 0.0};
Motion current_motion{};
Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation
double stop_dist{0.0}; // signed distance that is positive when car is before the stopline
Expand Down Expand Up @@ -271,11 +283,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro

/**
* @brief calculate control command based on the current control state
* @param [in] current_pose current ego pose
* @param [in] control_data control data
*/
Motion calcCtrlCmd(
const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data);
Motion calcCtrlCmd(const ControlData & control_data);

/**
* @brief publish control command
Expand Down Expand Up @@ -304,9 +314,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro

/**
* @brief calculate direction (forward or backward) that vehicle moves
* @param [in] nearest_idx nearest index on trajectory to vehicle
* @param [in] control_data data for control calculation
*/
enum Shift getCurrentShift(const size_t nearest_idx) const;
enum Shift getCurrentShift(const ControlData & control_data) const;

/**
* @brief filter acceleration command with limitation of acceleration and jerk, and slope
Expand Down Expand Up @@ -336,16 +346,16 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
* @param [in] motion delay compensated target motion
*/
Motion keepBrakeBeforeStop(
const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion,
const size_t nearest_idx) const;
const ControlData & control_data, const Motion & target_motion, const size_t nearest_idx) const;

/**
* @brief interpolate trajectory point that is nearest to vehicle
* @param [in] traj reference trajectory
* @param [in] point vehicle position
* @param [in] nearest_idx index of the trajectory point nearest to the vehicle position
*/
autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue(
std::pair<autoware_auto_planning_msgs::msg::TrajectoryPoint, size_t>
calcInterpolatedTrajPointAndSegment(
const autoware_auto_planning_msgs::msg::Trajectory & traj,
const geometry_msgs::msg::Pose & pose) const;

Expand All @@ -354,18 +364,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
* @param [in] current_motion current velocity and acceleration of the vehicle
* @param [in] delay_compensation_time predicted time delay
*/
double predictedVelocityInTargetPoint(
StateAfterDelay predictedStateAfterDelay(
const Motion current_motion, const double delay_compensation_time) const;

/**
* @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);
double applyVelocityFeedback(const ControlData & control_data);

/**
* @brief update variables for debugging about pitch
Expand All @@ -378,12 +384,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
/**
* @brief update variables for velocity and acceleration
* @param [in] ctrl_cmd latest calculated control command
* @param [in] current_pose current pose of the vehicle
* @param [in] control_data data for control calculation
*/
void updateDebugVelAcc(
const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose,
const ControlData & control_data);
void updateDebugVelAcc(const ControlData & control_data);
};
} // namespace autoware::motion::control::pid_longitudinal_controller

Expand Down
117 changes: 79 additions & 38 deletions control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,11 +58,23 @@ bool isValidTrajectory(const Trajectory & traj)
double calcStopDistance(
const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw)
{
const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points);

const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1;
const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
traj.points, current_pose, max_dist, max_yaw);
const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points);
const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1;
// if (end_idx == traj.points.size() - 1 && 1 >= int(end_idx - seg_idx)) {
// // Check the ego in front of the last point of trajectory or not
// const auto yaw = tier4_autoware_utils::getRPY(current_pose).z;
// const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw));
// const Eigen::Vector2d vehicle_to_end_vec(
// traj.points.back().pose.position.x - current_pose.position.x,
// traj.points.back().pose.position.y - current_pose.position.y);
// if (base_pose_vec.dot(vehicle_to_end_vec) < 0.0) {
// return -1.0 *
// tier4_autoware_utils::calcDistance3d(current_pose, traj.points.at(end_idx).pose);
// }
// return tier4_autoware_utils::calcDistance3d(current_pose, traj.points.at(end_idx).pose);
// }
const double signed_length_on_traj = motion_utils::calcSignedArcLength(
traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position,
std::min(end_idx, traj.points.size() - 2));
Expand Down Expand Up @@ -90,30 +102,13 @@ double getPitchByTraj(
if (trajectory.points.size() <= 1) {
return 0.0;
}
// find the trajectory point after wheelbase
const auto pose_after_distance =
findTrajectoryPoseAfterDistance(nearest_idx, wheel_base, trajectory);
TrajectoryPoint point_after_distance = trajectory.points.back();
point_after_distance.pose = pose_after_distance;

for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) {
const double dist = tier4_autoware_utils::calcDistance2d(
trajectory.points.at(nearest_idx), trajectory.points.at(i));
if (dist > wheel_base) {
// calculate pitch from trajectory between rear wheel (nearest) and front center (i)
return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i));
}
}

// close to goal
for (size_t i = trajectory.points.size() - 1; i > 0; --i) {
const double dist =
tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i));

if (dist > wheel_base) {
// calculate pitch from trajectory
// between wheelbase behind the end of trajectory (i) and the end of trajectory (back)
return calcElevationAngle(trajectory.points.at(i), trajectory.points.back());
}
}

// calculate pitch from trajectory between the beginning and end of trajectory
return calcElevationAngle(trajectory.points.at(0), trajectory.points.back());
return calcElevationAngle(trajectory.points.at(nearest_idx), point_after_distance);
}

double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to)
Expand All @@ -128,12 +123,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 @@ -142,17 +136,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 @@ -187,5 +175,58 @@ double applyDiffLimitFilter(
const double min_val = -max_val;
return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val);
}

geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
const size_t src_idx, const double distance,
const autoware_auto_planning_msgs::msg::Trajectory & trajectory)
{
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) {
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 +
ratio * (trajectory.points.at(i + 1).pose.position.x -
trajectory.points.at(i).pose.position.x);
p.position.y = trajectory.points.at(i).pose.position.y +
ratio * (trajectory.points.at(i + 1).pose.position.y -
trajectory.points.at(i).pose.position.y);
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;
}
return p;
}

TrajectoryPoint getExtendTrajectoryPoint(
const double extend_distance, const TrajectoryPoint & goal_point)
{
tf2::Transform map2goal;
tf2::fromMsg(goal_point.pose, map2goal);
tf2::Transform local_extend_point;
local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0));
tf2::Quaternion q;
q.setRPY(0, 0, 0);
local_extend_point.setRotation(q);
const auto map2extend_point = map2goal * local_extend_point;
geometry_msgs::msg::Pose extend_pose;
tf2::toMsg(map2extend_point, extend_pose);
TrajectoryPoint extend_trajectory_point;
extend_trajectory_point.pose = extend_pose;
extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps;
extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps;
extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2;
return extend_trajectory_point;
}

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

0 comments on commit fcc9756

Please sign in to comment.