Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[pull] main from autowarefoundation:main #49

Merged
merged 2 commits into from
Dec 21, 2022
Merged

[pull] main from autowarefoundation:main #49

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
febbc13
refactor(obstacle_cruise_planner): clean up the code (#2526)
takayuki5168 Dec 21, 2022
f33905a
chore(lidar_centerpoint): update maintainer (#2549)
knzo25 Dec 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions perception/lidar_centerpoint/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
<version>1.0.0</version>
<description>The lidar_centerpoint package</description>
<maintainer email="yusuke.muramatsu@tier4.jp">Yusuke Muramatsu</maintainer>
<maintainer email="kenzo.lobos@tier4.jp">Kenzo Lobos-Tsunekawa</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
Expand Down
58 changes: 39 additions & 19 deletions planning/obstacle_cruise_planner/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,30 +4,29 @@

The `obstacle_cruise_planner` package has following modules.

- obstacle stop planning
- inserting a stop point in the trajectory when there is a static obstacle on the trajectory.
- adaptive cruise planning
- sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory
- Stop planning
- stop when there is a static obstacle near the trajectory.
- Cruise planning
- slow down when there is a dynamic obstacle to cruise near the trajectory

## Interfaces

### Input topics

| Name | Type | Description |
| ----------------------------- | ----------------------------------------------- | --------------------------------- |
| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory |
| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory with smoothed velocity |
| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects |
| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry |
| Name | Type | Description |
| -------------------- | ----------------------------------------------- | ---------------- |
| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory |
| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects |
| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry |

### Output topics

| Name | Type | Description |
| ------------------------------ | ---------------------------------------------- | ------------------------------------- |
| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory |
| `~output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising |
| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit |
| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop |
| Name | Type | Description |
| ------------------------------- | ---------------------------------------------- | ------------------------------------- |
| `~/output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory |
| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising |
| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit |
| `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop |

## Design

Expand Down Expand Up @@ -176,16 +175,37 @@ This includes not only cruising a front vehicle, but also reacting a cut-in and
The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.

$$
d = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},
d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},
$$

assuming that $d$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration.
assuming that $d_rss$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration.
These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`.

| Parameter | Type | Description |
| --------------------------------- | ------ | ----------------------------------------------------------------------------- |
| `common.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] |
| `common.min_object_accel_for_rss` | double | front obstacle's acceleration [m/ss] |
| `common.min_ego_accel_for_rss` | double | ego's acceleration for RSS [m/ss] |
| `common.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss] |

The detailed formulation is as follows.

$$
d_{error} = d - d_{rss} \\
d_{normalized} = lpf(d_{error} / d_{obstacle}) \\
d_{quad, normalized} = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\
v_{pid} = pid(d_{quad, normalized}) \\
v_{add} = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\
v_{target} = max(v_{ego} + v_{add}, v_{min, cruise})
$$

| Variable | Description |
| ----------------- | --------------------------------------- |
| `d` | actual distane to obstacle |
| `d_{rss}` | ideal distance to obstacle based on RSS |
| `v_{min, cruise}` | `min_cruise_target_vel` |
| `w_{acc}` | `output_ratio_during_accel` |
| `lpf(val)` | apply low-pass filter to `val` |
| `pid(val)` | apply pid to `val` |

## Implementation

Expand Down
49 changes: 36 additions & 13 deletions planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,15 +3,18 @@
common:
planning_algorithm: "pid_base" # currently supported algorithm is "pid_base"

is_showing_debug_info: true
is_showing_debug_info: false
disable_stop_planning: false # true

# longitudinal info
idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s]
idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
safe_distance_margin : 6.0 # This is also used as a stop margin [m]
terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]

lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]

nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
min_behavior_stop_margin: 3.0 # [m]
Expand All @@ -25,7 +28,7 @@
bus: true
trailer: true
motorcycle: true
bicycle: false
bicycle: true
pedestrian: false

stop_obstacle_type:
Expand Down Expand Up @@ -62,7 +65,7 @@
stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle

ignored_outside_obstacle_type:
unknown: false
unknown: true
car: false
truck: false
bus: false
Expand All @@ -72,21 +75,41 @@
pedestrian: true

pid_based_planner:
# use_predicted_obstacle_pose: false
use_velocity_limit_based_planner: true
error_function_type: quadratic # choose from linear, quadratic

# PID gains to keep safe distance with the front vehicle
kp: 2.5
ki: 0.0
kd: 2.3
velocity_limit_based_planner:
# PID gains to keep safe distance with the front vehicle
kp: 10.0
ki: 0.0
kd: 2.0

min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]

enable_jerk_limit_to_output_acc: false

output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
disable_target_acceleration: true

velocity_insertion_based_planner:
kp_acc: 6.0
ki_acc: 0.0
kd_acc: 2.0

kp_jerk: 20.0
ki_jerk: 0.0
kd_jerk: 0.0

output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]

enable_jerk_limit_to_output_acc: true

min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
time_to_evaluate_rss: 0.0

lpf_cruise_gain: 0.2
lpf_normalized_error_cruise_dist_gain: 0.2

optimization_based_planner:
dense_resampling_time_interval: 0.2
Expand Down
86 changes: 58 additions & 28 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,37 +15,16 @@
#ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_
#define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_

#include "obstacle_cruise_planner/type_alias.hpp"
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"

#include <rclcpp/rclcpp.hpp>

#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "visualization_msgs/msg/marker_array.hpp"

#include <boost/optional.hpp>

#include <string>
#include <vector>

using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedPath;
using autoware_auto_perception_msgs::msg::Shape;

namespace
{
std::string toHexString(const unique_identifier_msgs::msg::UUID & id)
{
std::stringstream ss;
for (auto i = 0; i < 16; ++i) {
ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i];
}
return ss.str();
}
} // namespace

struct TargetObstacle
{
TargetObstacle(
Expand All @@ -59,7 +38,7 @@ struct TargetObstacle
velocity_reliable = true;
velocity = aligned_velocity;
classification = object.classification.at(0);
uuid = toHexString(object.object_id);
uuid = tier4_autoware_utils::toHexString(object.object_id);

predicted_paths.clear();
for (const auto & path : object.kinematics.predicted_paths) {
Expand All @@ -85,7 +64,7 @@ struct TargetObstacle
struct ObstacleCruisePlannerData
{
rclcpp::Time current_time;
autoware_auto_planning_msgs::msg::Trajectory traj;
Trajectory traj;
geometry_msgs::msg::Pose current_pose;
double current_vel;
double current_acc;
Expand All @@ -95,6 +74,50 @@ struct ObstacleCruisePlannerData

struct LongitudinalInfo
{
explicit LongitudinalInfo(rclcpp::Node & node)
{
max_accel = node.declare_parameter<double>("normal.max_acc");
min_accel = node.declare_parameter<double>("normal.min_acc");
max_jerk = node.declare_parameter<double>("normal.max_jerk");
min_jerk = node.declare_parameter<double>("normal.min_jerk");
limit_max_accel = node.declare_parameter<double>("limit.max_acc");
limit_min_accel = node.declare_parameter<double>("limit.min_acc");
limit_max_jerk = node.declare_parameter<double>("limit.max_jerk");
limit_min_jerk = node.declare_parameter<double>("limit.min_jerk");

idling_time = node.declare_parameter<double>("common.idling_time");
min_ego_accel_for_rss = node.declare_parameter<double>("common.min_ego_accel_for_rss");
min_object_accel_for_rss = node.declare_parameter<double>("common.min_object_accel_for_rss");

safe_distance_margin = node.declare_parameter<double>("common.safe_distance_margin");
terminal_safe_distance_margin =
node.declare_parameter<double>("common.terminal_safe_distance_margin");
}

void onParam(const std::vector<rclcpp::Parameter> & parameters)
{
tier4_autoware_utils::updateParam<double>(parameters, "normal.max_accel", max_accel);
tier4_autoware_utils::updateParam<double>(parameters, "normal.min_accel", min_accel);
tier4_autoware_utils::updateParam<double>(parameters, "normal.max_jerk", max_jerk);
tier4_autoware_utils::updateParam<double>(parameters, "normal.min_jerk", min_jerk);
tier4_autoware_utils::updateParam<double>(parameters, "limit.max_accel", limit_max_accel);
tier4_autoware_utils::updateParam<double>(parameters, "limit.min_accel", limit_min_accel);
tier4_autoware_utils::updateParam<double>(parameters, "limit.max_jerk", limit_max_jerk);
tier4_autoware_utils::updateParam<double>(parameters, "limit.min_jerk", limit_min_jerk);

tier4_autoware_utils::updateParam<double>(parameters, "common.idling_time", idling_time);
tier4_autoware_utils::updateParam<double>(
parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss);
tier4_autoware_utils::updateParam<double>(
parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss);

tier4_autoware_utils::updateParam<double>(
parameters, "common.safe_distance_margin", safe_distance_margin);
tier4_autoware_utils::updateParam<double>(
parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin);
}

// common parameter
double max_accel;
double min_accel;
double max_jerk;
Expand All @@ -103,9 +126,13 @@ struct LongitudinalInfo
double limit_min_accel;
double limit_max_jerk;
double limit_min_jerk;

// rss parameter
double idling_time;
double min_ego_accel_for_rss;
double min_object_accel_for_rss;

// distance margin
double safe_distance_margin;
double terminal_safe_distance_margin;
};
Expand All @@ -115,18 +142,21 @@ struct DebugData
std::vector<PredictedObject> intentionally_ignored_obstacles;
std::vector<TargetObstacle> obstacles_to_stop;
std::vector<TargetObstacle> obstacles_to_cruise;
visualization_msgs::msg::MarkerArray stop_wall_marker;
visualization_msgs::msg::MarkerArray cruise_wall_marker;
MarkerArray stop_wall_marker;
MarkerArray cruise_wall_marker;
std::vector<tier4_autoware_utils::Polygon2d> detection_polygons;
std::vector<geometry_msgs::msg::Point> collision_points;
};

struct EgoNearestParam
{
EgoNearestParam(const double arg_dist_threshold, const double arg_yaw_threshold)
: dist_threshold(arg_dist_threshold), yaw_threshold(arg_yaw_threshold)
EgoNearestParam() = default;
explicit EgoNearestParam(rclcpp::Node & node)
{
dist_threshold = node.declare_parameter<double>("ego_nearest_dist_threshold");
yaw_threshold = node.declare_parameter<double>("ego_nearest_yaw_threshold");
}

double dist_threshold;
double yaw_threshold;
};
Expand Down
Loading