Skip to content

Commit e0d53ce

Browse files
TomohitoAndoTomohitoAndo
committed
feat(run_out): use the predicted object's velocity and covariance for the collision detection (autowarefoundation#5672)
* feat(run_out): use the predicted object's velocity and covariance for the collision detection Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * chore: update readme Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix calculation Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> --------- Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
1 parent 202a178 commit e0d53ce

File tree

5 files changed

+64
-22
lines changed

5 files changed

+64
-22
lines changed

‎planning/behavior_velocity_run_out_module/README.md

+11-9
Original file line numberDiff line numberDiff line change
@@ -188,15 +188,17 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
188188
| `margin_ahead` | double | [m] ahead margin for detection area polygon |
189189
| `margin_behind` | double | [m] behind margin for detection area polygon |
190190

191-
| Parameter /dynamic_obstacle | Type | Description |
192-
| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- |
193-
| `min_vel_kmph` | double | [km/h] minimum velocity for dynamic obstacles |
194-
| `max_vel_kmph` | double | [km/h] maximum velocity for dynamic obstacles |
195-
| `diameter` | double | [m] diameter of obstacles. used for creating dynamic obstacles from points |
196-
| `height` | double | [m] height of obstacles. used for creating dynamic obstacles from points |
197-
| `max_prediction_time` | double | [sec] create predicted path until this time |
198-
| `time_step` | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path |
199-
| `points_interval` | double | [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method |
191+
| Parameter /dynamic_obstacle | Type | Description |
192+
| ------------------------------------ | ------ | ----------------------------------------------------------------------------------------------------------------------------- |
193+
| `use_mandatory_area` | double | [-] whether to use mandatory detection area |
194+
| `assume_fixed_velocity.enable` | double | [-] If enabled, the obstacle's velocity is assumed to be within the minimum and maximum velocity values specified below |
195+
| `assume_fixed_velocity.min_vel_kmph` | double | [km/h] minimum velocity for dynamic obstacles |
196+
| `assume_fixed_velocity.max_vel_kmph` | double | [km/h] maximum velocity for dynamic obstacles |
197+
| `diameter` | double | [m] diameter of obstacles. used for creating dynamic obstacles from points |
198+
| `height` | double | [m] height of obstacles. used for creating dynamic obstacles from points |
199+
| `max_prediction_time` | double | [sec] create predicted path until this time |
200+
| `time_step` | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path |
201+
| `points_interval` | double | [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method |
200202

201203
| Parameter /approaching | Type | Description |
202204
| ---------------------- | ------ | ----------------------------------------------------- |

‎planning/behavior_velocity_run_out_module/config/run_out.param.yaml

+11-8
Original file line numberDiff line numberDiff line change
@@ -21,14 +21,17 @@
2121

2222
# parameter to create abstracted dynamic obstacles
2323
dynamic_obstacle:
24-
use_mandatory_area: false # [-] whether to use mandatory detection area
25-
min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles
26-
max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles
27-
diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points
28-
height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points
29-
max_prediction_time: 10.0 # [sec] create predicted path until this time
30-
time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
31-
points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
24+
use_mandatory_area: false # [-] whether to use mandatory detection area
25+
assume_fixed_velocity:
26+
enable: false # [-] If enabled, the obstacle's velocity is assumed to be within the minimum and maximum velocity values specified below
27+
min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles
28+
max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles
29+
std_dev_multiplier: 1.96 # [-] min and max velocity of the obstacles are calculated from this value and covariance
30+
diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points
31+
height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points
32+
max_prediction_time: 10.0 # [sec] create predicted path until this time
33+
time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
34+
points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
3235

3336
# approach if ego has stopped in the front of the obstacle for a certain amount of time
3437
approaching:

‎planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp

+33-3
Original file line numberDiff line numberDiff line change
@@ -278,6 +278,31 @@ PointCloud2 concatPointCloud(
278278
return concat_points;
279279
}
280280

281+
void calculateMinAndMaxVelFromCovariance(
282+
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
283+
const double std_dev_multiplier, run_out_utils::DynamicObstacle & dynamic_obstacle)
284+
{
285+
const double x_velocity = std::abs(twist_with_covariance.twist.linear.x);
286+
const double y_velocity = std::abs(twist_with_covariance.twist.linear.y);
287+
const double x_variance = twist_with_covariance.covariance.at(0);
288+
const double y_variance = twist_with_covariance.covariance.at(7);
289+
const double x_std_dev = std::sqrt(x_variance);
290+
const double y_std_dev = std::sqrt(y_variance);
291+
292+
// calculate the min and max velocity using the standard deviation of twist
293+
// note that this assumes the covariance of x and y is zero
294+
const double min_x = std::max(0.0, x_velocity - std_dev_multiplier * x_std_dev);
295+
const double min_y = std::max(0.0, y_velocity - std_dev_multiplier * y_std_dev);
296+
const double min_velocity = std::hypot(min_x, min_y);
297+
298+
const double max_x = x_velocity + std_dev_multiplier * x_std_dev;
299+
const double max_y = y_velocity + std_dev_multiplier * y_std_dev;
300+
const double max_velocity = std::hypot(max_x, max_y);
301+
302+
dynamic_obstacle.min_velocity_mps = min_velocity;
303+
dynamic_obstacle.max_velocity_mps = max_velocity;
304+
}
305+
281306
} // namespace
282307

283308
DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(
@@ -294,9 +319,14 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForObject::createDynamicObsta
294319
DynamicObstacle dynamic_obstacle;
295320
dynamic_obstacle.pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
296321

297-
// TODO(Tomohito Ando): calculate velocity from covariance of predicted_object
298-
dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph);
299-
dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph);
322+
if (param_.assume_fixed_velocity) {
323+
dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph);
324+
dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph);
325+
} else {
326+
calculateMinAndMaxVelFromCovariance(
327+
predicted_object.kinematics.initial_twist_with_covariance, param_.std_dev_multiplier,
328+
dynamic_obstacle);
329+
}
300330
dynamic_obstacle.classifications = predicted_object.classification;
301331
dynamic_obstacle.shape = predicted_object.shape;
302332

‎planning/behavior_velocity_run_out_module/src/manager.cpp

+7-2
Original file line numberDiff line numberDiff line change
@@ -84,8 +84,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
8484
auto & p = planner_param_.dynamic_obstacle;
8585
const std::string ns_do = ns + ".dynamic_obstacle";
8686
p.use_mandatory_area = getOrDeclareParameter<bool>(node, ns_do + ".use_mandatory_area");
87-
p.min_vel_kmph = getOrDeclareParameter<double>(node, ns_do + ".min_vel_kmph");
88-
p.max_vel_kmph = getOrDeclareParameter<double>(node, ns_do + ".max_vel_kmph");
87+
p.assume_fixed_velocity =
88+
getOrDeclareParameter<bool>(node, ns_do + ".assume_fixed_velocity.enable");
89+
p.min_vel_kmph =
90+
getOrDeclareParameter<double>(node, ns_do + ".assume_fixed_velocity.min_vel_kmph");
91+
p.max_vel_kmph =
92+
getOrDeclareParameter<double>(node, ns_do + ".assume_fixed_velocity.max_vel_kmph");
93+
p.std_dev_multiplier = getOrDeclareParameter<double>(node, ns_do + ".std_dev_multiplier");
8994
p.diameter = getOrDeclareParameter<double>(node, ns_do + ".diameter");
9095
p.height = getOrDeclareParameter<double>(node, ns_do + ".height");
9196
p.max_prediction_time = getOrDeclareParameter<double>(node, ns_do + ".max_prediction_time");

‎planning/behavior_velocity_run_out_module/src/utils.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -109,11 +109,13 @@ struct Smoother
109109
struct DynamicObstacleParam
110110
{
111111
bool use_mandatory_area;
112+
bool assume_fixed_velocity;
112113

113114
float min_vel_kmph;
114115
float max_vel_kmph;
115116

116117
// parameter to convert points to dynamic obstacle
118+
float std_dev_multiplier;
117119
float diameter; // [m]
118120
float height; // [m]
119121
float max_prediction_time; // [sec]

0 commit comments

Comments
 (0)