Skip to content

Commit 49112d9

Browse files
TomohitoAndoTomohitoAndo
authored
feat(run_out): use the predicted object's velocity and covariance for the collision detection (#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 bc43dec commit 49112d9

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
@@ -287,6 +287,31 @@ PointCloud2 concatPointCloud(
287287
return concat_points;
288288
}
289289

290+
void calculateMinAndMaxVelFromCovariance(
291+
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
292+
const double std_dev_multiplier, run_out_utils::DynamicObstacle & dynamic_obstacle)
293+
{
294+
const double x_velocity = std::abs(twist_with_covariance.twist.linear.x);
295+
const double y_velocity = std::abs(twist_with_covariance.twist.linear.y);
296+
const double x_variance = twist_with_covariance.covariance.at(0);
297+
const double y_variance = twist_with_covariance.covariance.at(7);
298+
const double x_std_dev = std::sqrt(x_variance);
299+
const double y_std_dev = std::sqrt(y_variance);
300+
301+
// calculate the min and max velocity using the standard deviation of twist
302+
// note that this assumes the covariance of x and y is zero
303+
const double min_x = std::max(0.0, x_velocity - std_dev_multiplier * x_std_dev);
304+
const double min_y = std::max(0.0, y_velocity - std_dev_multiplier * y_std_dev);
305+
const double min_velocity = std::hypot(min_x, min_y);
306+
307+
const double max_x = x_velocity + std_dev_multiplier * x_std_dev;
308+
const double max_y = y_velocity + std_dev_multiplier * y_std_dev;
309+
const double max_velocity = std::hypot(max_x, max_y);
310+
311+
dynamic_obstacle.min_velocity_mps = min_velocity;
312+
dynamic_obstacle.max_velocity_mps = max_velocity;
313+
}
314+
290315
} // namespace
291316

292317
DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(
@@ -303,9 +328,14 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForObject::createDynamicObsta
303328
DynamicObstacle dynamic_obstacle;
304329
dynamic_obstacle.pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
305330

306-
// TODO(Tomohito Ando): calculate velocity from covariance of predicted_object
307-
dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph);
308-
dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph);
331+
if (param_.assume_fixed_velocity) {
332+
dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph);
333+
dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph);
334+
} else {
335+
calculateMinAndMaxVelFromCovariance(
336+
predicted_object.kinematics.initial_twist_with_covariance, param_.std_dev_multiplier,
337+
dynamic_obstacle);
338+
}
309339
dynamic_obstacle.classifications = predicted_object.classification;
310340
dynamic_obstacle.shape = predicted_object.shape;
311341

‎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
@@ -114,11 +114,13 @@ struct Smoother
114114
struct DynamicObstacleParam
115115
{
116116
bool use_mandatory_area;
117+
bool assume_fixed_velocity;
117118

118119
float min_vel_kmph;
119120
float max_vel_kmph;
120121

121122
// parameter to convert points to dynamic obstacle
123+
float std_dev_multiplier;
122124
float diameter; // [m]
123125
float height; // [m]
124126
float max_prediction_time; // [sec]

0 commit comments

Comments
 (0)