@@ -66,13 +66,25 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
66
66
double vel{0.0 };
67
67
double acc{0.0 };
68
68
};
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
+ };
70
79
enum class Shift { Forward = 0 , Reverse };
71
80
72
81
struct ControlData
73
82
{
74
83
bool is_far_from_trajectory{false };
84
+ autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{};
75
85
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 };
76
88
Motion current_motion{};
77
89
Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation
78
90
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
184
196
double m_min_jerk;
185
197
186
198
// 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;
188
202
std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr };
189
203
double m_max_pitch_rad;
190
204
double m_min_pitch_rad;
@@ -276,11 +290,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
276
290
277
291
/* *
278
292
* @brief calculate control command based on the current control state
279
- * @param [in] current_pose current ego pose
280
293
* @param [in] control_data control data
281
294
*/
282
- Motion calcCtrlCmd (
283
- const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data);
295
+ Motion calcCtrlCmd (const ControlData & control_data);
284
296
285
297
/* *
286
298
* @brief publish control command
@@ -309,9 +321,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
309
321
310
322
/* *
311
323
* @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
313
325
*/
314
- enum Shift getCurrentShift (const size_t nearest_idx ) const ;
326
+ enum Shift getCurrentShift (const ControlData & control_data ) const ;
315
327
316
328
/* *
317
329
* @brief filter acceleration command with limitation of acceleration and jerk, and slope
@@ -341,16 +353,16 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
341
353
* @param [in] motion delay compensated target motion
342
354
*/
343
355
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 ;
346
357
347
358
/* *
348
359
* @brief interpolate trajectory point that is nearest to vehicle
349
360
* @param [in] traj reference trajectory
350
361
* @param [in] point vehicle position
351
362
* @param [in] nearest_idx index of the trajectory point nearest to the vehicle position
352
363
*/
353
- autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue (
364
+ std::pair<autoware_auto_planning_msgs::msg::TrajectoryPoint, size_t >
365
+ calcInterpolatedTrajPointAndSegment (
354
366
const autoware_auto_planning_msgs::msg::Trajectory & traj,
355
367
const geometry_msgs::msg::Pose & pose) const ;
356
368
@@ -359,18 +371,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
359
371
* @param [in] current_motion current velocity and acceleration of the vehicle
360
372
* @param [in] delay_compensation_time predicted time delay
361
373
*/
362
- double predictedVelocityInTargetPoint (
374
+ StateAfterDelay predictedStateAfterDelay (
363
375
const Motion current_motion, const double delay_compensation_time) const ;
364
376
365
377
/* *
366
378
* @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
371
380
*/
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);
374
382
375
383
/* *
376
384
* @brief update variables for debugging about pitch
@@ -383,12 +391,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
383
391
/* *
384
392
* @brief update variables for velocity and acceleration
385
393
* @param [in] ctrl_cmd latest calculated control command
386
- * @param [in] current_pose current pose of the vehicle
387
394
* @param [in] control_data data for control calculation
388
395
*/
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);
392
397
393
398
double getTimeUnderControl ();
394
399
};
0 commit comments