Skip to content

Commit ff87583

Browse files
TomohitoAndoTomohitoAndo
authored
Merge pull request #1249 from tier4/cherry-pick-run-out-improve
feat(behavior_velocity_run_out): cherry pick feature improvements
2 parents bbeb8bc + f14f5ee commit ff87583

File tree

9 files changed

+171
-64
lines changed

9 files changed

+171
-64
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

+12-8
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
run_out:
44
detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points
55
use_partition_lanelet: true # [-] whether to use partition lanelet map data
6+
suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet:
67
specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates
78
stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin
89
passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin
@@ -21,14 +22,17 @@
2122

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

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

‎planning/behavior_velocity_run_out_module/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919

2020
<depend>autoware_auto_perception_msgs</depend>
2121
<depend>autoware_auto_planning_msgs</depend>
22+
<depend>behavior_velocity_crosswalk_module</depend>
2223
<depend>behavior_velocity_planner_common</depend>
2324
<depend>eigen</depend>
2425
<depend>geometry_msgs</depend>

‎planning/behavior_velocity_run_out_module/src/debug.hpp

+11-8
Original file line numberDiff line numberDiff line change
@@ -33,14 +33,17 @@ class DebugValues
3333
public:
3434
enum class TYPE {
3535
CALCULATION_TIME = 0,
36-
CALCULATION_TIME_COLLISION_CHECK = 1,
37-
LATERAL_DIST = 2,
38-
LONGITUDINAL_DIST_OBSTACLE = 3,
39-
LONGITUDINAL_DIST_COLLISION = 4,
40-
COLLISION_POS_FROM_EGO_FRONT = 5,
41-
STOP_DISTANCE = 6,
42-
NUM_OBSTACLES = 7,
43-
LATERAL_PASS_DIST = 8,
36+
CALCULATION_TIME_PATH_PROCESSING = 1,
37+
CALCULATION_TIME_OBSTACLE_CREATION = 2,
38+
CALCULATION_TIME_COLLISION_CHECK = 3,
39+
CALCULATION_TIME_PATH_PLANNING = 4,
40+
LATERAL_DIST = 5,
41+
LONGITUDINAL_DIST_OBSTACLE = 6,
42+
LONGITUDINAL_DIST_COLLISION = 7,
43+
COLLISION_POS_FROM_EGO_FRONT = 8,
44+
STOP_DISTANCE = 9,
45+
NUM_OBSTACLES = 10,
46+
LATERAL_PASS_DIST = 11,
4447
SIZE, // this is the number of enum elements
4548
};
4649

‎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

+8-2
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
5858
auto & p = planner_param_.run_out;
5959
p.detection_method = getOrDeclareParameter<std::string>(node, ns + ".detection_method");
6060
p.use_partition_lanelet = getOrDeclareParameter<bool>(node, ns + ".use_partition_lanelet");
61+
p.suppress_on_crosswalk = getOrDeclareParameter<bool>(node, ns + ".suppress_on_crosswalk");
6162
p.specify_decel_jerk = getOrDeclareParameter<bool>(node, ns + ".specify_decel_jerk");
6263
p.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
6364
p.passing_margin = getOrDeclareParameter<double>(node, ns + ".passing_margin");
@@ -84,8 +85,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
8485
auto & p = planner_param_.dynamic_obstacle;
8586
const std::string ns_do = ns + ".dynamic_obstacle";
8687
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");
88+
p.assume_fixed_velocity =
89+
getOrDeclareParameter<bool>(node, ns_do + ".assume_fixed_velocity.enable");
90+
p.min_vel_kmph =
91+
getOrDeclareParameter<double>(node, ns_do + ".assume_fixed_velocity.min_vel_kmph");
92+
p.max_vel_kmph =
93+
getOrDeclareParameter<double>(node, ns_do + ".assume_fixed_velocity.max_vel_kmph");
94+
p.std_dev_multiplier = getOrDeclareParameter<double>(node, ns_do + ".std_dev_multiplier");
8995
p.diameter = getOrDeclareParameter<double>(node, ns_do + ".diameter");
9096
p.height = getOrDeclareParameter<double>(node, ns_do + ".height");
9197
p.max_prediction_time = getOrDeclareParameter<double>(node, ns_do + ".max_prediction_time");

0 commit comments

Comments
 (0)