Skip to content

Commit 75bca30

Browse files
committed
feat(pid_longitudinal_controller): improve delay and slope compensation
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 8265e50 commit 75bca30

6 files changed

+326
-182
lines changed

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

+18-4
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828

2929
#include <cmath>
3030
#include <limits>
31+
#include <utility>
3132

3233
namespace autoware::motion::control::pid_longitudinal_controller
3334
{
@@ -60,11 +61,11 @@ double getPitchByPose(const Quaternion & quaternion);
6061
* @brief calculate pitch angle from trajectory on map
6162
* NOTE: there is currently no z information so this always returns 0.0
6263
* @param [in] trajectory input trajectory
63-
* @param [in] closest_idx nearest index to current vehicle position
64+
* @param [in] start_idx nearest index to current vehicle position
6465
* @param [in] wheel_base length of wheel base
6566
*/
6667
double getPitchByTraj(
67-
const Trajectory & trajectory, const size_t closest_idx, const double wheel_base);
68+
const Trajectory & trajectory, const size_t start_idx, const double wheel_base);
6869

6970
/**
7071
* @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and
@@ -88,7 +89,7 @@ Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, c
8889
* @param [in] point Interpolated point is nearest to this point.
8990
*/
9091
template <class T>
91-
TrajectoryPoint lerpTrajectoryPoint(
92+
std::pair<TrajectoryPoint, size_t> lerpTrajectoryPoint(
9293
const T & points, const Pose & pose, const double max_dist, const double max_yaw)
9394
{
9495
TrajectoryPoint interpolated_point;
@@ -107,6 +108,8 @@ TrajectoryPoint lerpTrajectoryPoint(
107108
points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
108109
interpolated_point.pose.position.y = interpolation::lerp(
109110
points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
111+
interpolated_point.pose.position.z = interpolation::lerp(
112+
points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio);
110113
interpolated_point.pose.orientation = lerpOrientation(
111114
points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
112115
interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
@@ -120,7 +123,7 @@ TrajectoryPoint lerpTrajectoryPoint(
120123
points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio);
121124
}
122125

123-
return interpolated_point;
126+
return std::make_pair(interpolated_point, seg_idx);
124127
}
125128

126129
/**
@@ -144,6 +147,17 @@ double applyDiffLimitFilter(
144147
double applyDiffLimitFilter(
145148
const double input_val, const double prev_val, const double dt, const double max_val,
146149
const double min_val);
150+
151+
/**
152+
* @brief calculate the projected pose after distance from the current index
153+
* @param [in] src_idx current index
154+
* @param [in] distance distance to project
155+
* @param [in] trajectory reference trajectory
156+
*/
157+
geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
158+
const size_t src_idx, const double distance,
159+
const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
160+
147161
} // namespace longitudinal_utils
148162
} // namespace autoware::motion::control::pid_longitudinal_controller
149163

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

+26-21
Original file line numberDiff line numberDiff line change
@@ -66,13 +66,25 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
6666
double vel{0.0};
6767
double acc{0.0};
6868
};
69-
69+
struct StateAfterDelay
70+
{
71+
StateAfterDelay(const double velocity, const double acceleration, const double distance)
72+
: vel(velocity), acc(acceleration), running_distance(distance)
73+
{
74+
}
75+
double vel{0.0};
76+
double acc{0.0};
77+
double running_distance{0.0};
78+
};
7079
enum class Shift { Forward = 0, Reverse };
7180

7281
struct ControlData
7382
{
7483
bool is_far_from_trajectory{false};
84+
autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{};
7585
size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx
86+
size_t target_idx{0};
87+
StateAfterDelay state_after_delay{0.0, 0.0, 0.0};
7688
Motion current_motion{};
7789
Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation
7890
double stop_dist{0.0}; // signed distance that is positive when car is before the stopline
@@ -184,7 +196,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
184196
double m_min_jerk;
185197

186198
// slope compensation
187-
bool m_use_traj_for_pitch;
199+
enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE };
200+
SlopeSource m_slope_source{SlopeSource::RAW_PITCH};
201+
double m_adaptive_trajectory_velocity_th;
188202
std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
189203
double m_max_pitch_rad;
190204
double m_min_pitch_rad;
@@ -276,11 +290,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
276290

277291
/**
278292
* @brief calculate control command based on the current control state
279-
* @param [in] current_pose current ego pose
280293
* @param [in] control_data control data
281294
*/
282-
Motion calcCtrlCmd(
283-
const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data);
295+
Motion calcCtrlCmd(const ControlData & control_data);
284296

285297
/**
286298
* @brief publish control command
@@ -309,9 +321,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
309321

310322
/**
311323
* @brief calculate direction (forward or backward) that vehicle moves
312-
* @param [in] nearest_idx nearest index on trajectory to vehicle
324+
* @param [in] control_data data for control calculation
313325
*/
314-
enum Shift getCurrentShift(const size_t nearest_idx) const;
326+
enum Shift getCurrentShift(const ControlData & control_data) const;
315327

316328
/**
317329
* @brief filter acceleration command with limitation of acceleration and jerk, and slope
@@ -341,16 +353,16 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
341353
* @param [in] motion delay compensated target motion
342354
*/
343355
Motion keepBrakeBeforeStop(
344-
const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion,
345-
const size_t nearest_idx) const;
356+
const ControlData & control_data, const Motion & target_motion, const size_t nearest_idx) const;
346357

347358
/**
348359
* @brief interpolate trajectory point that is nearest to vehicle
349360
* @param [in] traj reference trajectory
350361
* @param [in] point vehicle position
351362
* @param [in] nearest_idx index of the trajectory point nearest to the vehicle position
352363
*/
353-
autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue(
364+
std::pair<autoware_auto_planning_msgs::msg::TrajectoryPoint, size_t>
365+
calcInterpolatedTrajPointAndSegment(
354366
const autoware_auto_planning_msgs::msg::Trajectory & traj,
355367
const geometry_msgs::msg::Pose & pose) const;
356368

@@ -359,18 +371,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
359371
* @param [in] current_motion current velocity and acceleration of the vehicle
360372
* @param [in] delay_compensation_time predicted time delay
361373
*/
362-
double predictedVelocityInTargetPoint(
374+
StateAfterDelay predictedStateAfterDelay(
363375
const Motion current_motion, const double delay_compensation_time) const;
364376

365377
/**
366378
* @brief calculate velocity feedback with feed forward and pid controller
367-
* @param [in] target_motion reference velocity and acceleration. This acceleration will be used
368-
* as feed forward.
369-
* @param [in] dt time step to use
370-
* @param [in] current_vel current velocity of the vehicle
379+
* @param [in] control_data data for control calculation
371380
*/
372-
double applyVelocityFeedback(
373-
const Motion target_motion, const double dt, const double current_vel, const Shift & shift);
381+
double applyVelocityFeedback(const ControlData & control_data);
374382

375383
/**
376384
* @brief update variables for debugging about pitch
@@ -383,12 +391,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
383391
/**
384392
* @brief update variables for velocity and acceleration
385393
* @param [in] ctrl_cmd latest calculated control command
386-
* @param [in] current_pose current pose of the vehicle
387394
* @param [in] control_data data for control calculation
388395
*/
389-
void updateDebugVelAcc(
390-
const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose,
391-
const ControlData & control_data);
396+
void updateDebugVelAcc(const ControlData & control_data);
392397

393398
double getTimeUnderControl();
394399
};

control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml

+8-7
Original file line numberDiff line numberDiff line change
@@ -5,19 +5,19 @@
55
enable_smooth_stop: true
66
enable_overshoot_emergency: true
77
enable_large_tracking_error_emergency: true
8-
enable_slope_compensation: false
8+
enable_slope_compensation: true
99
enable_keep_stopped_until_steer_convergence: true
1010

1111
# state transition
1212
drive_state_stop_dist: 0.5
1313
drive_state_offset_stop_dist: 1.0
14-
stopping_state_stop_dist: 0.49
14+
stopping_state_stop_dist: 0.5
1515
stopped_state_entry_duration_time: 0.1
16-
stopped_state_entry_vel: 0.1
16+
stopped_state_entry_vel: 0.01
1717
stopped_state_entry_acc: 0.1
1818
emergency_state_overshoot_stop_dist: 1.5
1919
emergency_state_traj_trans_dev: 3.0
20-
emergency_state_traj_rot_dev: 0.7
20+
emergency_state_traj_rot_dev: 0.7854
2121

2222
# drive state
2323
kp: 1.0
@@ -40,7 +40,7 @@
4040

4141
# smooth stop state
4242
smooth_stop_max_strong_acc: -0.5
43-
smooth_stop_min_strong_acc: -1.0
43+
smooth_stop_min_strong_acc: -0.8
4444
smooth_stop_weak_acc: -0.3
4545
smooth_stop_weak_stop_acc: -0.8
4646
smooth_stop_strong_stop_acc: -3.4
@@ -69,8 +69,9 @@
6969
max_jerk: 2.0
7070
min_jerk: -5.0
7171

72-
# pitch
73-
use_trajectory_for_pitch_calculation: false
72+
# slope compensation
7473
lpf_pitch_gain: 0.95
74+
slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive
75+
adaptive_trajectory_velocity_th: 1.0
7576
max_pitch_rad: 0.1
7677
min_pitch_rad: -0.1

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

+33-5
Original file line numberDiff line numberDiff line change
@@ -84,25 +84,25 @@ double getPitchByPose(const Quaternion & quaternion_msg)
8484
}
8585

8686
double getPitchByTraj(
87-
const Trajectory & trajectory, const size_t nearest_idx, const double wheel_base)
87+
const Trajectory & trajectory, const size_t start_idx, const double wheel_base)
8888
{
8989
// cannot calculate pitch
9090
if (trajectory.points.size() <= 1) {
9191
return 0.0;
9292
}
9393

9494
const auto [prev_idx, next_idx] = [&]() {
95-
for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) {
95+
for (size_t i = start_idx + 1; i < trajectory.points.size(); ++i) {
9696
const double dist = tier4_autoware_utils::calcDistance2d(
97-
trajectory.points.at(nearest_idx), trajectory.points.at(i));
97+
trajectory.points.at(start_idx), trajectory.points.at(i));
9898
if (dist > wheel_base) {
9999
// calculate pitch from trajectory between rear wheel (nearest) and front center (i)
100-
return std::make_pair(nearest_idx, i);
100+
return std::make_pair(start_idx, i);
101101
}
102102
}
103103
// NOTE: The ego pose is close to the goal.
104104
return std::make_pair(
105-
std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1);
105+
std::min(start_idx, trajectory.points.size() - 2), trajectory.points.size() - 1);
106106
}();
107107

108108
return tier4_autoware_utils::calcElevationAngle(
@@ -168,5 +168,33 @@ double applyDiffLimitFilter(
168168
const double min_val = -max_val;
169169
return applyDiffLimitFilter(input_val, prev_val, dt, max_val, min_val);
170170
}
171+
172+
geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
173+
const size_t src_idx, const double distance,
174+
const autoware_auto_planning_msgs::msg::Trajectory & trajectory)
175+
{
176+
double remain_dist = distance;
177+
geometry_msgs::msg::Pose p = trajectory.points.back().pose;
178+
for (size_t i = src_idx; i < trajectory.points.size() - 1; ++i) {
179+
const double dist = tier4_autoware_utils::calcDistance3d(
180+
trajectory.points.at(i).pose, trajectory.points.at(i + 1).pose);
181+
if (remain_dist < dist) {
182+
if (remain_dist <= 0.0) {
183+
return trajectory.points.at(i).pose;
184+
}
185+
double ratio = remain_dist / dist;
186+
const auto p0 = trajectory.points.at(i).pose;
187+
const auto p1 = trajectory.points.at(i + 1).pose;
188+
p = trajectory.points.at(i).pose;
189+
p.position.x = interpolation::lerp(p0.position.x, p1.position.x, ratio);
190+
p.position.y = interpolation::lerp(p0.position.y, p1.position.y, ratio);
191+
p.position.z = interpolation::lerp(p0.position.z, p1.position.z, ratio);
192+
p.orientation = lerpOrientation(p0.orientation, p1.orientation, ratio);
193+
break;
194+
}
195+
remain_dist -= dist;
196+
}
197+
return p;
198+
}
171199
} // namespace longitudinal_utils
172200
} // namespace autoware::motion::control::pid_longitudinal_controller

0 commit comments

Comments
 (0)