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

fix(behavior_velocity_planner): fix rtc behavior in crosswalk module (#1296) #83

Merged
merged 1 commit into from
Jul 11, 2022
Merged

fix(behavior_velocity_planner): fix rtc behavior in crosswalk module (#1296) #83

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
2 changes: 1 addition & 1 deletion ...io_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk

# param for ego velocity
slow_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake
max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake
no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph)
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/config/crosswalk.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk

# param for ego velocity
slow_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake
max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake
no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph)
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class CrosswalkModule : public SceneModuleInterface
double stop_line_margin;
double stop_position_threshold;
// param for ego velocity
double slow_velocity;
float min_slow_down_velocity;
double max_slow_down_jerk;
double max_slow_down_accel;
double no_relax_velocity;
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
cp.stop_position_threshold = node.declare_parameter(ns + ".stop_position_threshold", 1.0);

// param for ego velocity
cp.slow_velocity = node.declare_parameter(ns + ".slow_velocity", 5.0 / 3.6);
cp.min_slow_down_velocity = node.declare_parameter(ns + ".min_slow_down_velocity", 5.0 / 3.6);
cp.max_slow_down_jerk = node.declare_parameter(ns + ".max_slow_down_jerk", -0.7);
cp.max_slow_down_accel = node.declare_parameter(ns + ".max_slow_down_accel", -2.5);
cp.no_relax_velocity = node.declare_parameter(ns + ".no_relax_velocity", 2.78);
Expand Down
39 changes: 11 additions & 28 deletions planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -363,20 +363,23 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto

setSafe(!nearest_stop_point);

const auto target_velocity =
nearest_stop_point ? calcTargetVelocity(nearest_stop_point.get().second, ego_path) : 0.0;

if (isActivated()) {
if (1e-3 < target_velocity) {
insertDecelPoint(nearest_stop_point.get(), target_velocity, *path);
if (!nearest_stop_point) {
return true;
}

const auto target_velocity = calcTargetVelocity(nearest_stop_point.get().second, ego_path);
insertDecelPoint(
nearest_stop_point.get(), std::max(planner_param_.min_slow_down_velocity, target_velocity),
*path);

return true;
}

if (nearest_stop_point) {
insertDecelPoint(nearest_stop_point.get(), target_velocity, *path);
insertDecelPoint(nearest_stop_point.get(), 0.0, *path);
} else if (rtc_stop_point) {
insertDecelPoint(rtc_stop_point.get(), target_velocity, *path);
insertDecelPoint(rtc_stop_point.get(), 0.0, *path);
}

RCLCPP_INFO_EXPRESSION(
Expand Down Expand Up @@ -442,13 +445,6 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea

bool found_pedestrians = false;
bool found_stuck_vehicle = false;
const bool external_slowdown = isTargetExternalInputStatus(CrosswalkStatus::SLOWDOWN);
const bool external_stop = isTargetExternalInputStatus(CrosswalkStatus::STOP);
const bool external_go = isTargetExternalInputStatus(CrosswalkStatus::GO);

if (external_go) {
return {};
}

const auto crosswalk_attention_range = getAttentionRange(ego_path);
const auto & ego_pos = planner_data_->current_pose.pose.position;
Expand Down Expand Up @@ -540,8 +536,7 @@ boost::optional<std::pair<size_t, PathPointWithLaneId>> CrosswalkModule::findNea
}
}

const auto need_to_stop =
found_pedestrians || found_stuck_vehicle || external_stop || external_slowdown;
const auto need_to_stop = found_pedestrians || found_stuck_vehicle;
if (!need_to_stop) {
return {};
}
Expand Down Expand Up @@ -646,18 +641,6 @@ void CrosswalkModule::insertDecelPoint(
float CrosswalkModule::calcTargetVelocity(
const PathPointWithLaneId & stop_point, const PathWithLaneId & ego_path) const
{
if (!isActivated()) {
return 0.0;
}

if (isTargetExternalInputStatus(CrosswalkStatus::SLOWDOWN)) {
return planner_param_.slow_velocity;
}

if (isTargetExternalInputStatus(CrosswalkStatus::STOP)) {
return 0.0;
}

const auto & max_jerk = planner_param_.max_slow_down_jerk;
const auto & max_accel = planner_param_.max_slow_down_accel;
const auto & ego_pos = planner_data_->current_pose.pose.position;
Expand Down