Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(pid_longitudinal_controller): improve delay and slope compensation #4712

Merged
merged 1 commit into from
Feb 9, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@

#include <cmath>
#include <limits>
#include <utility>

namespace autoware::motion::control::pid_longitudinal_controller
{
Expand Down Expand Up @@ -62,11 +63,11 @@ double getPitchByPose(const Quaternion & quaternion);
* @brief calculate pitch angle from trajectory on map
* NOTE: there is currently no z information so this always returns 0.0
* @param [in] trajectory input trajectory
* @param [in] closest_idx nearest index to current vehicle position
* @param [in] start_idx nearest index to current vehicle position
* @param [in] wheel_base length of wheel base
*/
double getPitchByTraj(
const Trajectory & trajectory, const size_t closest_idx, const double wheel_base);
const Trajectory & trajectory, const size_t start_idx, const double wheel_base);

/**
* @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and
Expand All @@ -82,7 +83,7 @@ Pose calcPoseAfterTimeDelay(
* @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 @@ -101,6 +102,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 = interpolation::lerpOrientation(
points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
Expand All @@ -114,7 +117,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 @@ -138,6 +141,17 @@ 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);
TakaHoribe marked this conversation as resolved.
Show resolved Hide resolved

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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,13 +66,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 @@ -184,7 +196,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
double m_min_jerk;

// slope compensation
bool m_use_traj_for_pitch;
enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE };
SlopeSource m_slope_source{SlopeSource::RAW_PITCH};
double m_adaptive_trajectory_velocity_th;
std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
double m_max_pitch_rad;
double m_min_pitch_rad;
Expand Down Expand Up @@ -276,11 +290,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 @@ -309,9 +321,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 @@ -341,16 +353,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 @@ -359,18 +371,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, const Shift & shift);
double applyVelocityFeedback(const ControlData & control_data);

/**
* @brief update variables for debugging about pitch
Expand All @@ -383,12 +391,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);

double getTimeUnderControl();
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,8 +69,9 @@
max_jerk: 2.0
min_jerk: -5.0

# pitch
use_trajectory_for_pitch_calculation: false
# slope compensation
lpf_pitch_gain: 0.95
slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive
adaptive_trajectory_velocity_th: 1.0
max_pitch_rad: 0.1
min_pitch_rad: -0.1
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2018-2021 Tier IV, Inc. All rights reserved.

Check notice on line 1 in control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Primitive Obsession

The ratio of primitive types in function arguments decreases from 72.00% to 67.86%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -84,25 +84,25 @@
}

double getPitchByTraj(
const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base)
const Trajectory & trajectory, const size_t start_idx, const double wheel_base)
{
// cannot calculate pitch
if (trajectory.points.size() <= 1) {
return 0.0;
}

const auto [prev_idx, next_idx] = [&]() {
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));
for (size_t i = start_idx + 1; i < trajectory.points.size(); ++i) {
const double dist = tier4_autoware_utils::calcDistance3d(
trajectory.points.at(start_idx), trajectory.points.at(i));
if (dist > wheel_base) {
// calculate pitch from trajectory between rear wheel (nearest) and front center (i)
return std::make_pair(nearest_idx, i);
return std::make_pair(start_idx, i);
}
}
// NOTE: The ego pose is close to the goal.
return std::make_pair(
std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1);
std::min(start_idx, trajectory.points.size() - 2), trajectory.points.size() - 1);
}();

return tier4_autoware_utils::calcElevationAngle(
Expand Down Expand Up @@ -158,5 +158,33 @@
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;
const auto p0 = trajectory.points.at(i).pose;
const auto p1 = trajectory.points.at(i + 1).pose;
p = trajectory.points.at(i).pose;
p.position.x = interpolation::lerp(p0.position.x, p1.position.x, ratio);
p.position.y = interpolation::lerp(p0.position.y, p1.position.y, ratio);
p.position.z = interpolation::lerp(p0.position.z, p1.position.z, ratio);
p.orientation = interpolation::lerpOrientation(p0.orientation, p1.orientation, ratio);
break;
}
remain_dist -= dist;
}
return p;
}
} // namespace longitudinal_utils
} // namespace autoware::motion::control::pid_longitudinal_controller
Loading
Loading