Skip to content

Commit 7a10058

Browse files
committed
use acceleration to calculate required trajectory length
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
1 parent 081109d commit 7a10058

File tree

5 files changed

+71
-34
lines changed

5 files changed

+71
-34
lines changed

planning/planning_validator/README.md

+22-12
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@ The following features are supported for trajectory validation and can have thre
1818
- **Steering angle rate** : invalid if the expected steering rate value is too large
1919
- **Velocity deviation** : invalid if the planning velocity is too far from the ego velocity
2020
- **Distance deviation** : invalid if the ego is too far from the trajectory
21+
- **Longitudinal distance deviation** : invalid if the trajectory is too far from ego in longitudinal direction
22+
- **Forward trajectory length** : invalid if the trajectory length is not enough to stop within a given deceleration
2123

2224
The following features are to be implemented.
2325

@@ -63,15 +65,23 @@ The following parameters can be set for the `planning_validator`:
6365

6466
The input trajectory is detected as invalid if the index exceeds the following thresholds.
6567

66-
| Name | Type | Description | Default value |
67-
| :-------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------------- | :------------ |
68-
| `thresholds.interval` | double | invalid threshold of the distance of two neighboring trajectory points [m] | 100.0 |
69-
| `thresholds.relative_angle` | double | invalid threshold of the relative angle of two neighboring trajectory points [rad] | 2.0 |
70-
| `thresholds.curvature` | double | invalid threshold of the curvature in each trajectory point [1/m] | 1.0 |
71-
| `thresholds.lateral_acc` | double | invalid threshold of the lateral acceleration in each trajectory point [m/ss] | 9.8 |
72-
| `thresholds.longitudinal_max_acc` | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point [m/ss] | 9.8 |
73-
| `thresholds.longitudinal_min_acc` | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point [m/ss] | -9.8 |
74-
| `thresholds.steering` | double | invalid threshold of the steering angle in each trajectory point [rad] | 1.414 |
75-
| `thresholds.steering_rate` | double | invalid threshold of the steering angle rate in each trajectory point [rad/s] | 10.0 |
76-
| `thresholds.velocity_deviation` | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego [m/s] | 100.0 |
77-
| `thresholds.distance_deviation` | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego [m] | 100.0 |
68+
| Name | Type | Description | Default value |
69+
| :------------------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------------- | :------------ |
70+
| `thresholds.interval` | double | invalid threshold of the distance of two neighboring trajectory points [m] | 100.0 |
71+
| `thresholds.relative_angle` | double | invalid threshold of the relative angle of two neighboring trajectory points [rad] | 2.0 |
72+
| `thresholds.curvature` | double | invalid threshold of the curvature in each trajectory point [1/m] | 1.0 |
73+
| `thresholds.lateral_acc` | double | invalid threshold of the lateral acceleration in each trajectory point [m/ss] | 9.8 |
74+
| `thresholds.longitudinal_max_acc` | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point [m/ss] | 9.8 |
75+
| `thresholds.longitudinal_min_acc` | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point [m/ss] | -9.8 |
76+
| `thresholds.steering` | double | invalid threshold of the steering angle in each trajectory point [rad] | 1.414 |
77+
| `thresholds.steering_rate` | double | invalid threshold of the steering angle rate in each trajectory point [rad/s] | 10.0 |
78+
| `thresholds.velocity_deviation` | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego [m/s] | 100.0 |
79+
| `thresholds.distance_deviation` | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego [m] | 100.0 |
80+
| `parameters.longitudinal_distance_deviation` | double | invalid threshold of the longitudinal distance deviation between the ego position and the trajectory [m] | 2.0 |
81+
82+
#### Parameters
83+
84+
For parameters used e.g. to calculate threshold.
85+
86+
| `parameters.forward_trajectory_length_acceleration` | double | This value is used to calculate required trajectory length. | -5.0 |
87+
| `parameters.forward_trajectory_length_margin` | double | A margin of the required trajectory length not to raise an error when the ego slightly exceeds the trajectory end point. | 2.0 |

planning/planning_validator/config/planning_validator.param.yaml

+10-1
Original file line numberDiff line numberDiff line change
@@ -27,4 +27,13 @@
2727
velocity_deviation: 100.0
2828
distance_deviation: 100.0
2929
longitudinal_distance_deviation: 1.0
30-
forward_trajectory_time_length: 3.0
30+
31+
parameters:
32+
# The required trajectory length is calculated as the distance needed
33+
# to stop from the current speed at this deceleration.
34+
forward_trajectory_length_acceleration: -3.0
35+
36+
# An error is raised if the required trajectory length is less than this distance.
37+
# Setting it to 0 means an error will occur if even slightly exceeding the end of the path,
38+
# therefore, a certain margin is necessary.
39+
forward_trajectory_length_margin: 2.0

planning/planning_validator/include/planning_validator/planning_validator.hpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@ using tier4_debug_msgs::msg::Float64Stamped;
4545

4646
struct ValidationParams
4747
{
48+
// thresholds
4849
double interval_threshold;
4950
double relative_angle_threshold;
5051
double curvature_threshold;
@@ -56,7 +57,10 @@ struct ValidationParams
5657
double velocity_deviation_threshold;
5758
double distance_deviation_threshold;
5859
double longitudinal_distance_deviation_threshold;
59-
double forward_trajectory_time_length_threshold;
60+
61+
// parameters
62+
double forward_trajectory_length_acceleration;
63+
double forward_trajectory_length_margin;
6064
};
6165

6266
class PlanningValidator : public rclcpp::Node

planning/planning_validator/msg/PlanningValidatorStatus.msg

+3-2
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ bool is_valid_steering_rate
1414
bool is_valid_velocity_deviation
1515
bool is_valid_distance_deviation
1616
bool is_valid_longitudinal_distance_deviation
17-
bool is_valid_forward_trajectory_time_length
17+
bool is_valid_forward_trajectory_length
1818

1919
# values
2020
int64 trajectory_size
@@ -29,6 +29,7 @@ float64 max_steering_rate
2929
float64 velocity_deviation
3030
float64 distance_deviation
3131
float64 longitudinal_distance_deviation
32-
float64 forward_trajectory_time_length
32+
float64 forward_trajectory_length_required
33+
float64 forward_trajectory_length_measured
3334

3435
int64 invalid_count

planning/planning_validator/src/planning_validator.cpp

+31-18
Original file line numberDiff line numberDiff line change
@@ -82,8 +82,12 @@ void PlanningValidator::setupParameters()
8282
p.distance_deviation_threshold = declare_parameter<double>(t + "distance_deviation");
8383
p.longitudinal_distance_deviation_threshold =
8484
declare_parameter<double>(t + "longitudinal_distance_deviation");
85-
p.forward_trajectory_time_length_threshold =
86-
declare_parameter<double>(t + "forward_trajectory_time_length");
85+
86+
const std::string ps = "parameters.";
87+
p.forward_trajectory_length_acceleration =
88+
declare_parameter<double>(ps + "forward_trajectory_length_acceleration");
89+
p.forward_trajectory_length_margin =
90+
declare_parameter<double>(ps + "forward_trajectory_length_margin");
8791
}
8892

8993
try {
@@ -161,10 +165,10 @@ void PlanningValidator::setupDiag()
161165
stat, validation_status_.is_valid_longitudinal_distance_deviation,
162166
"longitudinal distance deviation is too large");
163167
});
164-
d->add(ns + "forward_trajectory_time_length", [&](auto & stat) {
168+
d->add(ns + "forward_trajectory_length", [&](auto & stat) {
165169
setStatus(
166-
stat, validation_status_.is_valid_forward_trajectory_time_length,
167-
"trajectory time length is too short");
170+
stat, validation_status_.is_valid_forward_trajectory_length,
171+
"trajectory length is too short");
168172
});
169173
}
170174

@@ -299,7 +303,7 @@ void PlanningValidator::validate(const Trajectory & trajectory)
299303
s.is_valid_velocity_deviation = checkValidVelocityDeviation(trajectory);
300304
s.is_valid_distance_deviation = checkValidDistanceDeviation(trajectory);
301305
s.is_valid_longitudinal_distance_deviation = checkValidLongitudinalDistanceDeviation(trajectory);
302-
s.is_valid_forward_trajectory_time_length = checkValidForwardTrajectoryTimeLength(trajectory);
306+
s.is_valid_forward_trajectory_length = checkValidForwardTrajectoryTimeLength(trajectory);
303307

304308
// use resampled trajectory because the following metrics can not be evaluated for closed points.
305309
// Note: do not interpolate to keep original trajectory shape.
@@ -517,17 +521,28 @@ bool PlanningValidator::checkValidLongitudinalDistanceDeviation(const Trajectory
517521

518522
bool PlanningValidator::checkValidForwardTrajectoryTimeLength(const Trajectory & trajectory)
519523
{
520-
const auto forward_length = motion_utils::calcSignedArcLength(
521-
trajectory.points, current_kinematics_->pose.pose.position, trajectory.points.size() - 1);
522524
const auto ego_speed = std::abs(current_kinematics_->twist.twist.linear.x);
523-
if (ego_speed < 0.1) {
524-
return true; // Ego is stopped.
525+
if (ego_speed < 1.0 / 3.6) {
526+
return true; // Ego is almost stopped.
525527
}
526-
const auto forward_time_length = forward_length / std::max(ego_speed, 0.1);
527-
validation_status_.forward_trajectory_time_length = forward_time_length;
528528

529-
return validation_status_.forward_trajectory_time_length >
530-
validation_params_.forward_trajectory_time_length_threshold;
529+
const auto forward_length = motion_utils::calcSignedArcLength(
530+
trajectory.points, current_kinematics_->pose.pose.position, trajectory.points.size() - 1);
531+
532+
const auto tmp_all_length =
533+
motion_utils::calcSignedArcLength(trajectory.points, 0, trajectory.points.size() - 1);
534+
535+
std::cerr << "forward_length = " << forward_length << ", tmp_all_length = " << tmp_all_length
536+
<< std::endl;
537+
538+
const auto acc = validation_params_.forward_trajectory_length_acceleration;
539+
const auto forward_length_required = ego_speed * ego_speed / (2.0 * std::abs(acc)) -
540+
validation_params_.forward_trajectory_length_margin;
541+
542+
validation_status_.forward_trajectory_length_required = forward_length_required;
543+
validation_status_.forward_trajectory_length_measured = forward_length;
544+
545+
return forward_length > forward_length_required;
531546
}
532547

533548
bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
@@ -537,7 +552,7 @@ bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
537552
s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc &&
538553
s.is_valid_steering && s.is_valid_steering_rate && s.is_valid_velocity_deviation &&
539554
s.is_valid_distance_deviation && s.is_valid_longitudinal_distance_deviation &&
540-
s.is_valid_forward_trajectory_time_length;
555+
s.is_valid_forward_trajectory_length;
541556
}
542557

543558
void PlanningValidator::displayStatus()
@@ -567,9 +582,7 @@ void PlanningValidator::displayStatus()
567582
warn(
568583
s.is_valid_longitudinal_distance_deviation,
569584
"planning trajectory is too far from ego in longitudinal direction!!");
570-
warn(
571-
s.is_valid_forward_trajectory_time_length,
572-
"planning trajectory forward time length is not enough!!");
585+
warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!");
573586
}
574587

575588
} // namespace planning_validator

0 commit comments

Comments
 (0)