Skip to content

Commit

Permalink
po
Browse files Browse the repository at this point in the history 
Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
@yuki-takagi-66
yuki-takagi-66 committed Sep 5, 2024
1 parent 71b4b8d commit cfca541
Show file tree
Hide file tree
Showing 2 changed files with 22 additions and 27 deletions.
41 changes: 18 additions & 23 deletions ...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,11 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
recordTime(2);

// Calculate stop point with margin
const auto default_stop_pose = getDefaultStopPose(*path, first_path_point_on_crosswalk);
const auto default_stop_pose_opt = getDefaultStopPose(*path, first_path_point_on_crosswalk);
if (!default_stop_pose_opt) {
RCLCPP_DEBUG(logger_, "Failure to get default_stop_pose");
return {};
}

// Resample path sparsely for less computation cost
constexpr double resample_interval = 4.0;
Expand All @@ -253,12 +257,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
// Decide to stop for crosswalk users
const auto stop_factor_for_crosswalk_users = checkStopForCrosswalkUsers(
*path, sparse_resample_path, first_path_point_on_crosswalk, last_path_point_on_crosswalk,
default_stop_pose);
*default_stop_pose_opt);

// Decide to stop for stuck vehicle
const auto stop_factor_for_stuck_vehicles = checkStopForStuckVehicles(
sparse_resample_path, objects_ptr->objects, first_path_point_on_crosswalk,
last_path_point_on_crosswalk, default_stop_pose);
last_path_point_on_crosswalk, *default_stop_pose_opt);

// Get nearest stop factor
const auto nearest_stop_factor =
Expand All @@ -270,13 +274,13 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto

// Set distance
// NOTE: If no stop point is inserted, distance to the virtual stop line has to be calculated.
setDistanceToStop(*path, default_stop_pose, nearest_stop_factor);
setDistanceToStop(*path, *default_stop_pose_opt, nearest_stop_factor);

// plan Go/Stop
if (isActivated()) {
planGo(*path, nearest_stop_factor);
} else {
planStop(*path, nearest_stop_factor, default_stop_pose, stop_reason);
planStop(*path, nearest_stop_factor, *default_stop_pose_opt, stop_reason);
}
recordTime(4);

Expand Down Expand Up @@ -314,15 +318,15 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
const geometry_msgs::msg::Point & first_path_point_on_crosswalk,
const geometry_msgs::msg::Point & last_path_point_on_crosswalk,
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose)
const geometry_msgs::msg::Pose & default_stop_pose)
{
const auto & ego_pos = planner_data_->current_odometry->pose.position;
const double ego_vel = planner_data_->current_velocity->twist.linear.x;
const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;

const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
const auto dist_ego_to_stop =
calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position);
calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose.position);

// Calculate attention range for crosswalk
const auto crosswalk_attention_range = getAttentionRange(
Expand Down Expand Up @@ -400,9 +404,7 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(

if (stop_at_stop_line) {
// Stop at the stop line
if (default_stop_pose) {
return createStopFactor(*default_stop_pose, stop_factor_points);
}
return createStopFactor(default_stop_pose, stop_factor_points);
} else {
const auto stop_pose = calcLongitudinalOffsetPose(
ego_path.points, nearest_stop_info->first,
Expand Down Expand Up @@ -946,14 +948,10 @@ std::optional<StopFactor> CrosswalkModule::checkStopForStuckVehicles(
const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
const geometry_msgs::msg::Point & first_path_point_on_crosswalk,
const geometry_msgs::msg::Point & last_path_point_on_crosswalk,
const std::optional<geometry_msgs::msg::Pose> & stop_pose)
const geometry_msgs::msg::Pose & stop_pose)
{
const auto & p = planner_param_;

if (!stop_pose) {
return {};
}

// skip stuck vehicle checking for crosswalk which is in intersection.
if (!p.enable_stuck_check_in_intersection) {
std::string turn_direction = road_.attributeOr("turn_direction", "else");
Expand Down Expand Up @@ -1004,7 +1002,7 @@ std::optional<StopFactor> CrosswalkModule::checkStopForStuckVehicles(

const auto & ego_pos = planner_data_->current_odometry->pose.position;
const double dist_ego2stop =
calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position);
calcSignedArcLength(ego_path.points, ego_pos, stop_pose.position);
const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop);
const double dist_to_ego =
calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos);
Expand Down Expand Up @@ -1246,15 +1244,13 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon(
}

void CrosswalkModule::setDistanceToStop(
const PathWithLaneId & ego_path,
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose,
const PathWithLaneId & ego_path, const geometry_msgs::msg::Pose & default_stop_pose,
const std::optional<StopFactor> & stop_factor)
{
// calculate stop position
const auto stop_pos = [&]() -> std::optional<geometry_msgs::msg::Point> {
if (stop_factor) return stop_factor->stop_pose.position;
if (default_stop_pose) return default_stop_pose->position;
return std::nullopt;
return default_stop_pose.position;
}();

// Set distance
Expand Down Expand Up @@ -1282,12 +1278,11 @@ void CrosswalkModule::planGo(

void CrosswalkModule::planStop(
PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason)
const geometry_msgs::msg::Pose & default_stop_pose, StopReason * stop_reason)
{
const auto stop_factor = [&]() -> std::optional<StopFactor> {
if (nearest_stop_factor) return *nearest_stop_factor;
if (default_stop_pose) return createStopFactor(*default_stop_pose);
return std::nullopt;
return createStopFactor(default_stop_pose);
}();

if (!stop_factor) {
Expand Down
8 changes: 4 additions & 4 deletions ...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -352,13 +352,13 @@ class CrosswalkModule : public SceneModuleInterface
const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
const geometry_msgs::msg::Point & first_path_point_on_crosswalk,
const geometry_msgs::msg::Point & last_path_point_on_crosswalk,
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose);
const geometry_msgs::msg::Pose & default_stop_pose);

std::optional<StopFactor> checkStopForStuckVehicles(
const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
const geometry_msgs::msg::Point & first_path_point_on_crosswalk,
const geometry_msgs::msg::Point & last_path_point_on_crosswalk,
const std::optional<geometry_msgs::msg::Pose> & stop_pose);
const geometry_msgs::msg::Pose & stop_pose);

std::optional<double> findEgoPassageDirectionAlongPath(
const PathWithLaneId & sparse_resample_path) const;
Expand All @@ -376,14 +376,14 @@ class CrosswalkModule : public SceneModuleInterface

void setDistanceToStop(
const PathWithLaneId & ego_path,
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose,
const geometry_msgs::msg::Pose & default_stop_pose,
const std::optional<StopFactor> & stop_factor);

void planGo(PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor) const;

void planStop(
PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason);
const geometry_msgs::msg::Pose & default_stop_pose, StopReason * stop_reason);

// minor functions
std::pair<double, double> getAttentionRange(
Expand Down

0 comments on commit cfca541

Please sign in to comment.