Skip to content

Commit

Permalink
feat(behavior_velocity_planner): remove stop_reason
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Dec 2, 2024
1 parent 9fac554 commit 1d7fd6d
Show file tree
Hide file tree
Showing 42 changed files with 89 additions and 416 deletions.
10 changes: 4 additions & 6 deletions ...velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ class BlindSpotModule : public SceneModuleInterface
* @brief plan go-stop velocity at traffic crossing with collision check between reference path
* and object predicted path
*/
bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
bool modifyPathVelocity(PathWithLaneId * path) override;

visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
std::vector<autoware::motion_utils::VirtualWall> createVirtualWalls() override;
Expand All @@ -133,20 +133,18 @@ class BlindSpotModule : public SceneModuleInterface
// Parameter

void initializeRTCStatus();
BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason);
BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path);
// setSafe(), setDistance()
void setRTCStatus(
const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path);
template <typename Decision>
void setRTCStatusByDecision(
const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path);
// stop/GO
void reactRTCApproval(
const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason);
void reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path);
template <typename Decision>
void reactRTCApprovalByDecision(
const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path,
StopReason * stop_reason);
const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path);

std::optional<InterpolatedPathInfo> generateInterpolatedPathInfo(
const tier4_planning_msgs::msg::PathWithLaneId & input_path) const;
Expand Down
22 changes: 5 additions & 17 deletions .../behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,7 @@ void BlindSpotModule::setRTCStatusByDecision(
template <typename T>
void BlindSpotModule::reactRTCApprovalByDecision(
[[maybe_unused]] const T & decision,
[[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path,
[[maybe_unused]] StopReason * stop_reason)
[[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path)
{
static_assert("Unsupported type passed to reactRTCApprovalByDecision");
}
Expand All @@ -53,8 +52,7 @@ void BlindSpotModule::setRTCStatusByDecision(
template <>
void BlindSpotModule::reactRTCApprovalByDecision(
[[maybe_unused]] const InternalError & decision,
[[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path,
[[maybe_unused]] StopReason * stop_reason)
[[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path)
{
return;
}
Expand All @@ -73,8 +71,7 @@ void BlindSpotModule::setRTCStatusByDecision(
template <>
void BlindSpotModule::reactRTCApprovalByDecision(
[[maybe_unused]] const OverPassJudge & decision,
[[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path,
[[maybe_unused]] StopReason * stop_reason)
[[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path)
{
return;
}
Expand All @@ -95,20 +92,15 @@ void BlindSpotModule::setRTCStatusByDecision(

template <>
void BlindSpotModule::reactRTCApprovalByDecision(
const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path,
StopReason * stop_reason)
const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path)
{
if (!isActivated()) {
constexpr double stop_vel = 0.0;
planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);

tier4_planning_msgs::msg::StopFactor stop_factor;
const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
stop_factor.stop_pose = stop_pose;
stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
planning_utils::appendStopReason(stop_factor, stop_reason);
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
}
Expand All @@ -131,19 +123,15 @@ void BlindSpotModule::setRTCStatusByDecision(

template <>
void BlindSpotModule::reactRTCApprovalByDecision(
const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason)
const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path)
{
if (!isActivated()) {
constexpr double stop_vel = 0.0;
planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);

tier4_planning_msgs::msg::StopFactor stop_factor;
const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
stop_factor.stop_pose = stop_pose;
stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
planning_utils::appendStopReason(stop_factor, stop_reason);
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
}
Expand Down
18 changes: 7 additions & 11 deletions ...ning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,7 @@ void BlindSpotModule::initializeRTCStatus()
setDistance(std::numeric_limits<double>::lowest());
}

BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * path)
{
if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) {
return OverPassJudge{"already over the pass judge line. no plan needed."};
Expand Down Expand Up @@ -160,26 +159,23 @@ void BlindSpotModule::setRTCStatus(
decision);
}

void BlindSpotModule::reactRTCApproval(
const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason)
void BlindSpotModule::reactRTCApproval(const BlindSpotDecision & decision, PathWithLaneId * path)
{
std::visit(
VisitorSwitch{[&](const auto & sub_decision) {
reactRTCApprovalByDecision(sub_decision, path, stop_reason);
}},
VisitorSwitch{
[&](const auto & sub_decision) { reactRTCApprovalByDecision(sub_decision, path); }},
decision);
}

bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path)
{
debug_data_ = DebugData();
*stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT);

initializeRTCStatus();
const auto decision = modifyPathVelocityDetail(path, stop_reason);
const auto decision = modifyPathVelocityDetail(path);
const auto & input_path = *path;
setRTCStatus(decision, input_path);
reactRTCApproval(decision, path, stop_reason);
reactRTCApproval(decision, path);

return true;
}
Expand Down
8 changes: 3 additions & 5 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 @@ -201,7 +201,7 @@ CrosswalkModule::CrosswalkModule(
node.create_publisher<tier4_debug_msgs::msg::StringStamped>("~/debug/collision_info", 1);
}

bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path)
{
if (path->points.size() < 2) {
RCLCPP_DEBUG(logger_, "Do not interpolate because path size is less than 2.");
Expand All @@ -213,7 +213,6 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
logger_, planner_param_.show_processing_time,
"=========== module_id: %ld ===========", module_id_);

*stop_reason = planning_utils::initializeStopReason(StopReason::CROSSWALK);
const auto & ego_pos = planner_data_->current_odometry->pose.position;
const auto objects_ptr = planner_data_->predicted_objects;

Expand Down Expand Up @@ -274,7 +273,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
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);
}
recordTime(4);

Expand Down Expand Up @@ -1353,7 +1352,7 @@ 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 std::optional<geometry_msgs::msg::Pose> & default_stop_pose)
{
// Calculate stop factor
auto stop_factor = [&]() -> std::optional<StopFactor> {
Expand All @@ -1376,7 +1375,6 @@ void CrosswalkModule::planStop(

// Plan stop
insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path);
planning_utils::appendStopReason(*stop_factor, stop_reason);
velocity_factor_.set(
ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose,
VelocityFactor::UNKNOWN);
Expand Down
4 changes: 2 additions & 2 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 @@ -336,7 +336,7 @@ class CrosswalkModule : public SceneModuleInterface
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock);

bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
bool modifyPathVelocity(PathWithLaneId * path) override;

visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
autoware::motion_utils::VirtualWalls createVirtualWalls() override;
Expand Down Expand Up @@ -399,7 +399,7 @@ class CrosswalkModule : public SceneModuleInterface

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 std::optional<geometry_msgs::msg::Pose> & default_stop_pose);

// minor functions
std::pair<double, double> getAttentionRange(
Expand Down
20 changes: 1 addition & 19 deletions .../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,15 +47,14 @@ DetectionAreaModule::DetectionAreaModule(
velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA);
}

bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
{
// Store original path
const auto original_path = *path;

// Reset data
debug_data_ = DebugData();
debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
*stop_reason = planning_utils::initializeStopReason(StopReason::DETECTION_AREA);

// Find obstacles in detection area
const auto obstacle_points = detection_area::get_obstacle_points(
Expand Down Expand Up @@ -184,28 +183,11 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason *

// Create StopReason
{
StopFactor stop_factor{};
stop_factor.stop_pose = stop_point->second;
stop_factor.stop_factor_points = obstacle_points;
planning_utils::appendStopReason(stop_factor, stop_reason);
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_point->second,
VelocityFactor::UNKNOWN);
}

// Create legacy StopReason
{
const double stop_path_point_distance = autoware::motion_utils::calcSignedArcLength(
path->points, 0, stop_pose.position, stop_point->first);

if (
!first_stop_path_point_distance_ ||
stop_path_point_distance < first_stop_path_point_distance_.value()) {
debug_data_.first_stop_pose = stop_point->second;
first_stop_path_point_distance_ = stop_path_point_distance;
}
}

return true;
}
} // namespace autoware::behavior_velocity_planner
2 changes: 1 addition & 1 deletion .../behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ class DetectionAreaModule : public SceneModuleInterface
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock);

bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
bool modifyPathVelocity(PathWithLaneId * path) override;

visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
autoware::motion_utils::VirtualWalls createVirtualWalls() override;
Expand Down
Loading

0 comments on commit 1d7fd6d

Please sign in to comment.