Skip to content

Commit

Permalink
add abandon function
Browse files Browse the repository at this point in the history 
checkConsistency() is not supported yet

Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
@yuki-takagi-66
yuki-takagi-66 committed May 27, 2024
1 parent 9d71cce commit e22e541
Show file tree
Hide file tree
Showing 4 changed files with 167 additions and 93 deletions.
8 changes: 5 additions & 3 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
View file
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
41 changes: 40 additions & 1 deletion planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,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 @@ -333,6 +334,44 @@ class PlannerInterface
double lpf_gain_dist_to_slow_down;
};
SlowDownParam slow_down_param_;
struct StopParam
{
struct ObstacleSpecificParams
{
double limit_min_acc;
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.";
const ObstacleSpecificParams default_param{longitudinal_info.limit_min_accel, false};
type_specified_param_list.emplace("default", default_param);
for (const auto & type_map : types_maps) {
if (node.declare_parameter<bool>(
param_prefix + "use_type_specified_params." + type_map.second)) {
const ObstacleSpecificParams param{
node.declare_parameter<double>(param_prefix + "limit_min_acc." + type_map.second),
node.declare_parameter<bool>(param_prefix + "abandon_to_stop." + type_map.second)};
type_specified_param_list.emplace(type_map.second, param);
}
}
}
std::string getParamType(ObjectClassification label)
{
const auto type_str = types_maps.at(label.label);
if (type_specified_param_list.count(type_str) != 0) {
return type_str;
}
return "default";
}
};
StopParam stop_param_;
double moving_object_speed_threshold;
double moving_object_hysteresis_range;
std::vector<SlowDownOutput> prev_slow_down_output_;
Expand Down
12 changes: 9 additions & 3 deletions planning/obstacle_cruise_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -815,6 +815,12 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
}
slow_down_condition_counter_.removeCounterUnlessUpdated();

std::sort(
stop_obstacles.begin(), stop_obstacles.end(),
[](const StopObstacle & o1, const StopObstacle & o2) {
return o1.dist_to_collide_on_decimated_traj < o2.dist_to_collide_on_decimated_traj;
});

// Check target obstacles' consistency
checkConsistency(objects.header.stamp, objects, stop_obstacles);

Expand Down Expand Up @@ -1242,9 +1248,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
199 changes: 113 additions & 86 deletions planning/obstacle_cruise_planner/src/planner_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -259,9 +259,78 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_,
"stop planning");

// Get Closest Stop Obstacle
const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles);
if (!closest_stop_obstacle) {
std::optional<StopObstacle> determined_stop_obstacle{};
std::optional<double> determined_zero_vel_dist{};
std::optional<double> determined_desired_margin{};
// std::optional<bool> will_collide_with_obstacle{};
for (const auto & type_specified_param : stop_param_.type_specified_param_list) {
// stop_obstacles had been sorted by dist_to_collide_on_decimated_traj
for (const auto & stop_obstacle : stop_obstacles) {
if (stop_param_.getParamType(stop_obstacle.classification) != type_specified_param.first) {
continue;
}
const auto ego_segment_idx =
ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
const double dist_to_collide_on_ref_traj =
motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) +
stop_obstacle.dist_to_collide_on_decimated_traj;

const double desired_margin = [&]() {
const double margin_from_obstacle =
calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle);
// Use terminal margin (terminal_safe_distance_margin) for obstacle stop
const auto ref_traj_length = motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, planner_data.traj_points.size() - 1);
if (dist_to_collide_on_ref_traj > ref_traj_length) {
return longitudinal_info_.terminal_safe_distance_margin;
}

// If behavior stop point is ahead of the closest_obstacle_stop point within a certain
// margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance
const auto closest_behavior_stop_idx =
motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1);
if (closest_behavior_stop_idx) {
const double closest_behavior_stop_dist_on_ref_traj = motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, *closest_behavior_stop_idx);
const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj -
(dist_to_collide_on_ref_traj - margin_from_obstacle);
if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) {
return min_behavior_stop_margin_;
}
}
return margin_from_obstacle;
}();

// calc stop point against the obstacle
double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin);
if (suppress_sudden_obstacle_stop_) {
const double acceptable_stop_dist =
calcMinimumDistanceToStop(
planner_data.ego_vel, longitudinal_info_.limit_max_accel,
type_specified_param.second.limit_min_acc) +
motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, planner_data.ego_pose.position);

if (acceptable_stop_dist > candidate_zero_vel_dist) {
if (type_specified_param.second.abandon_to_stop && acceptable_stop_dist > dist_to_collide_on_ref_traj) {
RCLCPP_WARN(
rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"),
"[Cruise] abandon to stop against %s object", type_specified_param.first.c_str());
continue;
}
candidate_zero_vel_dist = acceptable_stop_dist;
}
}
if (!determined_zero_vel_dist || candidate_zero_vel_dist < determined_zero_vel_dist) {
determined_zero_vel_dist = candidate_zero_vel_dist;
determined_stop_obstacle = stop_obstacle;
determined_desired_margin = desired_margin;
break;
}
}
}

if (!determined_zero_vel_dist) {
// delete marker
const auto markers =
motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
Expand All @@ -271,117 +340,75 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
return planner_data.traj_points;
}

const auto ego_segment_idx =
ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
const double dist_to_collide_on_ref_traj =
motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) +
closest_stop_obstacle->dist_to_collide_on_decimated_traj;

const double margin_from_obstacle_considering_behavior_module = [&]() {
const double margin_from_obstacle =
calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle);
// Use terminal margin (terminal_safe_distance_margin) for obstacle stop
const auto ref_traj_length = motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, planner_data.traj_points.size() - 1);
if (dist_to_collide_on_ref_traj > ref_traj_length) {
return longitudinal_info_.terminal_safe_distance_margin;
}

// If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin
// we set closest_obstacle_stop_distance to closest_behavior_stop_distance
const auto closest_behavior_stop_idx =
motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1);
if (closest_behavior_stop_idx) {
const double closest_behavior_stop_dist_on_ref_traj =
motion_utils::calcSignedArcLength(planner_data.traj_points, 0, *closest_behavior_stop_idx);
const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj -
(dist_to_collide_on_ref_traj - margin_from_obstacle);
if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) {
return min_behavior_stop_margin_;
}
}
return margin_from_obstacle;
}();

// Generate Output Trajectory
const auto [zero_vel_dist, will_collide_with_obstacle] = [&]() {
double candidate_zero_vel_dist =
std::max(0.0, dist_to_collide_on_ref_traj - margin_from_obstacle_considering_behavior_module);

// Check feasibility to stop
if (suppress_sudden_obstacle_stop_) {
// Calculate feasible stop margin (Check the feasibility)
const double feasible_stop_dist =
calcMinimumDistanceToStop(
planner_data.ego_vel, longitudinal_info_.limit_max_accel,
longitudinal_info_.limit_min_accel) +
motion_utils::calcSignedArcLength(
planner_data.traj_points, 0, planner_data.ego_pose.position);

if (candidate_zero_vel_dist < feasible_stop_dist) {
candidate_zero_vel_dist = feasible_stop_dist;
return std::make_pair(candidate_zero_vel_dist, true);
}
}

// Hold previous stop distance if necessary
// Hold previous stop distance if necessary
if (
std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold &&
prev_stop_distance_info_) {
// NOTE: We assume that the current trajectory's front point is ahead of the previous
// trajectory's front point.
const size_t traj_front_point_prev_seg_idx =
motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
prev_stop_distance_info_->first, planner_data.traj_points.front().pose);
const double diff_dist_front_points = motion_utils::calcSignedArcLength(
prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position,
traj_front_point_prev_seg_idx);

const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
if (
std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold &&
prev_stop_distance_info_) {
// NOTE: We assume that the current trajectory's front point is ahead of the previous
// trajectory's front point.
const size_t traj_front_point_prev_seg_idx =
motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
prev_stop_distance_info_->first, planner_data.traj_points.front().pose);
const double diff_dist_front_points = motion_utils::calcSignedArcLength(
prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position,
traj_front_point_prev_seg_idx);

const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
if (
std::abs(prev_zero_vel_dist - candidate_zero_vel_dist) <
longitudinal_info_.hold_stop_distance_threshold) {
candidate_zero_vel_dist = prev_zero_vel_dist;
}
std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) <
longitudinal_info_.hold_stop_distance_threshold) {
determined_zero_vel_dist.value() = prev_zero_vel_dist;
}
return std::make_pair(candidate_zero_vel_dist, false);
}();
}

// Insert stop point
auto output_traj_points = planner_data.traj_points;
const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points);
const auto zero_vel_idx =
motion_utils::insertStopPoint(0, determined_zero_vel_dist.value(), output_traj_points);
if (zero_vel_idx) {
// virtual wall marker for stop obstacle
const auto markers = motion_utils::createStopVirtualWallMarker(
output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0,
abs_ego_offset, "", planner_data.is_driving_forward);
tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle);
debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle);

// Publish Stop Reason
const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
const auto stop_reasons_msg =
makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle);
makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle);
stop_reasons_pub_->publish(stop_reasons_msg);
velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose));

// Publish if ego vehicle collides with the obstacle with a limit acceleration
// Publish if ego vehicle will over run against the stop point with a limit acceleration
const bool will_over_run = determined_stop_obstacle->dist_to_collide_on_decimated_traj -
determined_zero_vel_dist.value() <
determined_desired_margin.value();
const auto stop_speed_exceeded_msg =
createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle);
createStopSpeedExceededMsg(planner_data.current_time, will_over_run);
stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg);

prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist);
prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value());
}

stop_planning_debug_info_.set(
StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE,
closest_stop_obstacle->dist_to_collide_on_decimated_traj);
determined_stop_obstacle->dist_to_collide_on_decimated_traj);
stop_planning_debug_info_.set(
StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity);
StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY,
determined_stop_obstacle->velocity);

// compatible implementation to the exsiting code is the choice 1, but should we change it to the
// choice2?
// choice 1
stop_planning_debug_info_.set(
StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE,
margin_from_obstacle_considering_behavior_module);
StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, determined_desired_margin.value());
// choice 2
// stop_planning_debug_info_.set(
// StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE,
// determined_stop_obstacle->dist_to_collide_on_decimated_traj -
// determined_zero_vel_dist.value());

stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0);
stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0);

Expand Down

0 comments on commit e22e541

Please sign in to comment.