Skip to content

Commit

Permalink
feat(obstacle_cruise)!: type specified stop deccel limit and enabling…
Browse files Browse the repository at this point in the history
… abandon to stop (#7121)

* add abandon function

Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
yuki-takagi-66 authored and KhalilSelyan committed Jul 22, 2024
1 parent f7050fd commit 5b18333
Show file tree
Hide file tree
Showing 8 changed files with 271 additions and 144 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -218,3 +218,15 @@
lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity
lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path
lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start
stop:
type_specified_params:
labels: # For the listed types, the node try to read the following type specified values
- "default"
- "unknown"
# default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined.
# limit_min_acc: common_param.yaml/limit.min_acc
unknown:
limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred.
sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop".
sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop".
abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit.
Original file line number Diff line number Diff line change
Expand Up @@ -113,14 +113,15 @@ struct StopObstacle : public TargetObstacleInterface
{
StopObstacle(
const std::string & arg_uuid, const rclcpp::Time & arg_stamp,
const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape,
const double arg_lon_velocity, const double arg_lat_velocity,
const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose,
const Shape & arg_shape, const double arg_lon_velocity, const double arg_lat_velocity,
const geometry_msgs::msg::Point arg_collision_point,
const double arg_dist_to_collide_on_decimated_traj)
: TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
shape(arg_shape),
collision_point(arg_collision_point),
dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj)
dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj),
classification(object_classification)
{
}
Shape shape;
Expand All @@ -129,6 +130,7 @@ struct StopObstacle : public TargetObstacleInterface
// calculateMarginFromObstacleOnCurve() and should be removed as it can be
// replaced by ”dist_to_collide_on_decimated_traj”
double dist_to_collide_on_decimated_traj;
ObjectClassification classification;
};

struct CruiseObstacle : public TargetObstacleInterface
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -272,8 +272,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
bool is_driving_forward_{true};
bool enable_slow_down_planning_{false};

// previous closest obstacle
std::shared_ptr<StopObstacle> prev_closest_stop_obstacle_ptr_{nullptr};
std::vector<StopObstacle> prev_closest_stop_obstacles_{};

std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@
#include "tier4_autoware_utils/ros/update_param.hpp"
#include "tier4_autoware_utils/system/stop_watch.hpp"

#include <algorithm>
#include <limits>
#include <memory>
#include <optional>
#include <string>
Expand All @@ -42,7 +44,8 @@ class PlannerInterface
vehicle_info_(vehicle_info),
ego_nearest_param_(ego_nearest_param),
debug_data_ptr_(debug_data_ptr),
slow_down_param_(SlowDownParam(node))
slow_down_param_(SlowDownParam(node)),
stop_param_(StopParam(node, longitudinal_info))
{
stop_reasons_pub_ = node.create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
velocity_factors_pub_ =
Expand Down Expand Up @@ -91,6 +94,7 @@ class PlannerInterface
updateCommonParam(parameters);
updateCruiseParam(parameters);
slow_down_param_.onParam(parameters);
stop_param_.onParam(parameters, longitudinal_info_);
}

Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const
Expand Down Expand Up @@ -333,6 +337,84 @@ class PlannerInterface
double lpf_gain_dist_to_slow_down;
};
SlowDownParam slow_down_param_;
struct StopParam
{
struct ObstacleSpecificParams
{
double limit_min_acc;
double sudden_object_acc_threshold;
double sudden_object_dist_threshold;
bool abandon_to_stop;
};
const std::unordered_map<uint8_t, std::string> types_maps = {
{ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"},
{ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"},
{ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"},
{ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}};
std::unordered_map<std::string, ObstacleSpecificParams> type_specified_param_list;
explicit StopParam(rclcpp::Node & node, const LongitudinalInfo & longitudinal_info)
{
const std::string param_prefix = "stop.type_specified_params.";
std::vector<std::string> obstacle_labels{"default"};
obstacle_labels =
node.declare_parameter<std::vector<std::string>>(param_prefix + "labels", obstacle_labels);

for (const auto & type_str : obstacle_labels) {
if (type_str != "default") {
ObstacleSpecificParams param{
node.declare_parameter<double>(param_prefix + type_str + ".limit_min_acc"),
node.declare_parameter<double>(
param_prefix + type_str + ".sudden_object_acc_threshold"),
node.declare_parameter<double>(
param_prefix + type_str + ".sudden_object_dist_threshold"),
node.declare_parameter<bool>(param_prefix + type_str + ".abandon_to_stop")};

param.sudden_object_acc_threshold =
std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel);
param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold);

type_specified_param_list.emplace(type_str, param);
}
}
}
void onParam(
const std::vector<rclcpp::Parameter> & parameters, const LongitudinalInfo & longitudinal_info)
{
const std::string param_prefix = "stop.type_specified_params.";
for (auto & [type_str, param] : type_specified_param_list) {
if (type_str == "default") {
continue;
}
tier4_autoware_utils::updateParam<double>(
parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc);
tier4_autoware_utils::updateParam<double>(
parameters, param_prefix + type_str + ".sudden_object_acc_threshold",
param.sudden_object_acc_threshold);
tier4_autoware_utils::updateParam<double>(
parameters, param_prefix + type_str + ".sudden_object_dist_threshold",
param.sudden_object_dist_threshold);
tier4_autoware_utils::updateParam<bool>(
parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop);

param.sudden_object_acc_threshold =
std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel);
param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold);
}
}
std::string getParamType(const ObjectClassification label)
{
const auto type_str = types_maps.at(label.label);
if (type_specified_param_list.count(type_str) == 0) {
return "default";
}
return type_str;
}
ObstacleSpecificParams getParam(const ObjectClassification label)
{
return type_specified_param_list.at(getParamType(label));
}
};
StopParam stop_param_;
double moving_object_speed_threshold;
double moving_object_hysteresis_range;
std::vector<SlowDownOutput> prev_slow_down_output_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,7 @@ PoseWithStamp getCurrentObjectPose(
const PredictedObject & predicted_object, const rclcpp::Time & obj_base_time,
const rclcpp::Time & current_time, const bool use_prediction);

std::optional<StopObstacle> getClosestStopObstacle(
const std::vector<StopObstacle> & stop_obstacles);
std::vector<StopObstacle> getClosestStopObstacles(const std::vector<StopObstacle> & stop_obstacles);

template <class T>
size_t getIndexWithLongitudinalOffset(
Expand Down
70 changes: 28 additions & 42 deletions planning/obstacle_cruise_planner/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1253,9 +1253,9 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
}

const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
return StopObstacle{
obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape,
tangent_vel, normal_vel, collision_point->first, collision_point->second};
return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification,
obstacle.pose, obstacle.shape, tangent_vel,
normal_vel, collision_point->first, collision_point->second};
}

std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacle(
Expand Down Expand Up @@ -1386,50 +1386,36 @@ void ObstacleCruisePlannerNode::checkConsistency(
const rclcpp::Time & current_time, const PredictedObjects & predicted_objects,
std::vector<StopObstacle> & stop_obstacles)
{
const auto current_closest_stop_obstacle =
obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles);

if (!prev_closest_stop_obstacle_ptr_) {
if (current_closest_stop_obstacle) {
prev_closest_stop_obstacle_ptr_ =
std::make_shared<StopObstacle>(*current_closest_stop_obstacle);
for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) {
const auto predicted_object_itr = std::find_if(
predicted_objects.objects.begin(), predicted_objects.objects.end(),
[&prev_closest_stop_obstacle](const PredictedObject & po) {
return tier4_autoware_utils::toHexString(po.object_id) == prev_closest_stop_obstacle.uuid;
});
// If previous closest obstacle disappear from the perception result, do nothing anymore.
if (predicted_object_itr == predicted_objects.objects.end()) {
continue;
}
return;
}

const auto predicted_object_itr = std::find_if(
predicted_objects.objects.begin(), predicted_objects.objects.end(),
[&](PredictedObject predicted_object) {
return tier4_autoware_utils::toHexString(predicted_object.object_id) ==
prev_closest_stop_obstacle_ptr_->uuid;
});
// If previous closest obstacle disappear from the perception result, do nothing anymore.
if (predicted_object_itr == predicted_objects.objects.end()) {
return;
}

const auto is_disappeared_from_stop_obstacle = std::none_of(
stop_obstacles.begin(), stop_obstacles.end(),
[&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; });
if (is_disappeared_from_stop_obstacle) {
// re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop"
// condition is satisfied
const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds();
if (
predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) {
stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_);
return;
const auto is_disappeared_from_stop_obstacle = std::none_of(
stop_obstacles.begin(), stop_obstacles.end(),
[&prev_closest_stop_obstacle](const StopObstacle & so) {
return so.uuid == prev_closest_stop_obstacle.uuid;
});
if (is_disappeared_from_stop_obstacle) {
// re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop"
// condition is satisfied
const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds();
if (
predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x <
behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise &&
elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) {
stop_obstacles.push_back(prev_closest_stop_obstacle);
}
}
}

if (current_closest_stop_obstacle) {
prev_closest_stop_obstacle_ptr_ =
std::make_shared<StopObstacle>(*current_closest_stop_obstacle);
} else {
prev_closest_stop_obstacle_ptr_ = nullptr;
}
prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles);
}

bool ObstacleCruisePlannerNode::isObstacleCrossing(
Expand Down
Loading

0 comments on commit 5b18333

Please sign in to comment.