Skip to content

Commit

Permalink
feat(run_out): use the predicted object's velocity and covariance for…
Browse files Browse the repository at this point in the history 
… the collision detection (#5672)

* feat(run_out): use the predicted object's velocity and covariance for the collision detection

Signed-off-by: Tomohito Ando <[email protected]>

* chore: update readme

Signed-off-by: Tomohito Ando <[email protected]>

* fix calculation

Signed-off-by: Tomohito Ando <[email protected]>

---------

Signed-off-by: Tomohito Ando <[email protected]>
  • Loading branch information
@TomohitoAndo
TomohitoAndo authored Nov 26, 2023
1 parent bc43dec commit 49112d9
Show file tree
Hide file tree
Showing 5 changed files with 64 additions and 22 deletions.
20 changes: 11 additions & 9 deletions planning/behavior_velocity_run_out_module/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,15 +188,17 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
| `margin_ahead` | double | [m] ahead margin for detection area polygon |
| `margin_behind` | double | [m] behind margin for detection area polygon |

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

| Parameter /approaching | Type | Description |
| ---------------------- | ------ | ----------------------------------------------------- |
Expand Down
19 changes: 11 additions & 8 deletions planning/behavior_velocity_run_out_module/config/run_out.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,14 +21,17 @@

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

# approach if ego has stopped in the front of the obstacle for a certain amount of time
approaching:
Expand Down
36 changes: 33 additions & 3 deletions planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -287,6 +287,31 @@ PointCloud2 concatPointCloud(
return concat_points;
}

void calculateMinAndMaxVelFromCovariance(
const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
const double std_dev_multiplier, run_out_utils::DynamicObstacle & dynamic_obstacle)
{
const double x_velocity = std::abs(twist_with_covariance.twist.linear.x);
const double y_velocity = std::abs(twist_with_covariance.twist.linear.y);
const double x_variance = twist_with_covariance.covariance.at(0);
const double y_variance = twist_with_covariance.covariance.at(7);
const double x_std_dev = std::sqrt(x_variance);
const double y_std_dev = std::sqrt(y_variance);

// calculate the min and max velocity using the standard deviation of twist
// note that this assumes the covariance of x and y is zero
const double min_x = std::max(0.0, x_velocity - std_dev_multiplier * x_std_dev);
const double min_y = std::max(0.0, y_velocity - std_dev_multiplier * y_std_dev);
const double min_velocity = std::hypot(min_x, min_y);

const double max_x = x_velocity + std_dev_multiplier * x_std_dev;
const double max_y = y_velocity + std_dev_multiplier * y_std_dev;
const double max_velocity = std::hypot(max_x, max_y);

dynamic_obstacle.min_velocity_mps = min_velocity;
dynamic_obstacle.max_velocity_mps = max_velocity;
}

} // namespace

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

// TODO(Tomohito Ando): calculate velocity from covariance of predicted_object
dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph);
dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph);
if (param_.assume_fixed_velocity) {
dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph);
dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph);
} else {
calculateMinAndMaxVelFromCovariance(
predicted_object.kinematics.initial_twist_with_covariance, param_.std_dev_multiplier,
dynamic_obstacle);
}
dynamic_obstacle.classifications = predicted_object.classification;
dynamic_obstacle.shape = predicted_object.shape;

Expand Down
9 changes: 7 additions & 2 deletions planning/behavior_velocity_run_out_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,8 +84,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
auto & p = planner_param_.dynamic_obstacle;
const std::string ns_do = ns + ".dynamic_obstacle";
p.use_mandatory_area = getOrDeclareParameter<bool>(node, ns_do + ".use_mandatory_area");
p.min_vel_kmph = getOrDeclareParameter<double>(node, ns_do + ".min_vel_kmph");
p.max_vel_kmph = getOrDeclareParameter<double>(node, ns_do + ".max_vel_kmph");
p.assume_fixed_velocity =
getOrDeclareParameter<bool>(node, ns_do + ".assume_fixed_velocity.enable");
p.min_vel_kmph =
getOrDeclareParameter<double>(node, ns_do + ".assume_fixed_velocity.min_vel_kmph");
p.max_vel_kmph =
getOrDeclareParameter<double>(node, ns_do + ".assume_fixed_velocity.max_vel_kmph");
p.std_dev_multiplier = getOrDeclareParameter<double>(node, ns_do + ".std_dev_multiplier");
p.diameter = getOrDeclareParameter<double>(node, ns_do + ".diameter");
p.height = getOrDeclareParameter<double>(node, ns_do + ".height");
p.max_prediction_time = getOrDeclareParameter<double>(node, ns_do + ".max_prediction_time");
Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_velocity_run_out_module/src/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,11 +114,13 @@ struct Smoother
struct DynamicObstacleParam
{
bool use_mandatory_area;
bool assume_fixed_velocity;

float min_vel_kmph;
float max_vel_kmph;

// parameter to convert points to dynamic obstacle
float std_dev_multiplier;
float diameter; // [m]
float height; // [m]
float max_prediction_time; // [sec]
Expand Down

0 comments on commit 49112d9

Please sign in to comment.