Skip to content

Commit

Permalink
Merge pull request #1249 from tier4/cherry-pick-run-out-improve
Browse files Browse the repository at this point in the history 
feat(behavior_velocity_run_out): cherry pick feature improvements
  • Loading branch information
@TomohitoAndo
TomohitoAndo authored Apr 12, 2024
2 parents bbeb8bc + f14f5ee commit ff87583
Show file tree
Hide file tree
Showing 9 changed files with 171 additions and 64 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
20 changes: 12 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 @@ -3,6 +3,7 @@
run_out:
detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points
use_partition_lanelet: true # [-] whether to use partition lanelet map data
suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet:
specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates
stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin
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
Expand All @@ -21,14 +22,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
1 change: 1 addition & 0 deletions planning/behavior_velocity_run_out_module/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>behavior_velocity_crosswalk_module</depend>
<depend>behavior_velocity_planner_common</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
Expand Down
19 changes: 11 additions & 8 deletions planning/behavior_velocity_run_out_module/src/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,17 @@ class DebugValues
public:
enum class TYPE {
CALCULATION_TIME = 0,
CALCULATION_TIME_COLLISION_CHECK = 1,
LATERAL_DIST = 2,
LONGITUDINAL_DIST_OBSTACLE = 3,
LONGITUDINAL_DIST_COLLISION = 4,
COLLISION_POS_FROM_EGO_FRONT = 5,
STOP_DISTANCE = 6,
NUM_OBSTACLES = 7,
LATERAL_PASS_DIST = 8,
CALCULATION_TIME_PATH_PROCESSING = 1,
CALCULATION_TIME_OBSTACLE_CREATION = 2,
CALCULATION_TIME_COLLISION_CHECK = 3,
CALCULATION_TIME_PATH_PLANNING = 4,
LATERAL_DIST = 5,
LONGITUDINAL_DIST_OBSTACLE = 6,
LONGITUDINAL_DIST_COLLISION = 7,
COLLISION_POS_FROM_EGO_FRONT = 8,
STOP_DISTANCE = 9,
NUM_OBSTACLES = 10,
LATERAL_PASS_DIST = 11,
SIZE, // this is the number of enum elements
};

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 @@ -278,6 +278,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 @@ -294,9 +319,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
10 changes: 8 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 @@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
auto & p = planner_param_.run_out;
p.detection_method = getOrDeclareParameter<std::string>(node, ns + ".detection_method");
p.use_partition_lanelet = getOrDeclareParameter<bool>(node, ns + ".use_partition_lanelet");
p.suppress_on_crosswalk = getOrDeclareParameter<bool>(node, ns + ".suppress_on_crosswalk");
p.specify_decel_jerk = getOrDeclareParameter<bool>(node, ns + ".specify_decel_jerk");
p.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
p.passing_margin = getOrDeclareParameter<double>(node, ns + ".passing_margin");
Expand All @@ -84,8 +85,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
Loading

0 comments on commit ff87583

Please sign in to comment.