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.

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

refactor(blind_spot): devide scene into Categories and act accordingly #7110

Merged
merged 2 commits into from
May 27, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
src/debug.cpp
src/manager.cpp
src/scene.cpp
src/decisions.cpp
)

ament_auto_package(INSTALL_TO_SHARE config)
54 changes: 2 additions & 52 deletions planning/behavior_velocity_blind_spot_module/src/debug.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,56 +70,17 @@

return msg;
}

Check notice on line 73 in planning/behavior_velocity_blind_spot_module/src/debug.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Excess Number of Function Arguments

createPoseMarkerArray is no longer above the threshold for number of arguments. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
visualization_msgs::msg::MarkerArray createPoseMarkerArray(
const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns,
const int64_t id, const double r, const double g, const double b)
{
visualization_msgs::msg::MarkerArray msg;

if (state == StateMachine::State::STOP) {
visualization_msgs::msg::Marker marker_line{};
marker_line.header.frame_id = "map";
marker_line.ns = ns + "_line";
marker_line.id = id;
marker_line.lifetime = rclcpp::Duration::from_seconds(0.3);
marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP;
marker_line.action = visualization_msgs::msg::Marker::ADD;
marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0);
marker_line.scale = createMarkerScale(0.1, 0.0, 0.0);
marker_line.color = createMarkerColor(r, g, b, 0.999);

const double yaw = tf2::getYaw(pose.orientation);

const double a = 3.0;
geometry_msgs::msg::Point p0;
p0.x = pose.position.x - a * std::sin(yaw);
p0.y = pose.position.y + a * std::cos(yaw);
p0.z = pose.position.z;
marker_line.points.push_back(p0);

geometry_msgs::msg::Point p1;
p1.x = pose.position.x + a * std::sin(yaw);
p1.y = pose.position.y - a * std::cos(yaw);
p1.z = pose.position.z;
marker_line.points.push_back(p1);

msg.markers.push_back(marker_line);
}

return msg;
}

} // namespace

motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls()
{
motion_utils::VirtualWalls virtual_walls;

if (!isActivated() && !is_over_pass_judge_line_) {
if (debug_data_.virtual_wall_pose) {
motion_utils::VirtualWall wall;
wall.text = "blind_spot";
wall.pose = debug_data_.virtual_wall_pose;
wall.pose = debug_data_.virtual_wall_pose.value();
wall.ns = std::to_string(module_id_) + "_";
virtual_walls.push_back(wall);
}
Expand All @@ -130,19 +91,8 @@
{
visualization_msgs::msg::MarkerArray debug_marker_array;

const auto state = state_machine_.getState();
const auto now = this->clock_->now();

appendMarkerArray(
createPoseMarkerArray(
debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0),
&debug_marker_array, now);

appendMarkerArray(
createPoseMarkerArray(
debug_data_.judge_point_pose, state, "judge_point_pose", module_id_, 1.0, 1.0, 0.5),
&debug_marker_array, now);

appendMarkerArray(
createLaneletPolygonsMarkerArray(
debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5),
Expand Down
154 changes: 154 additions & 0 deletions planning/behavior_velocity_blind_spot_module/src/decisions.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,154 @@
// Copyright 2024 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "scene.hpp"

#include <motion_utils/trajectory/trajectory.hpp>

namespace behavior_velocity_planner
{

/*
* for default
*/
template <typename T>
void BlindSpotModule::setRTCStatusByDecision(
const T &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
static_assert("Unsupported type passed to setRTCStatus");
return;
}

template <typename T>
void BlindSpotModule::reactRTCApprovalByDecision(
[[maybe_unused]] const T & decision,
[[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
[[maybe_unused]] StopReason * stop_reason)
{
static_assert("Unsupported type passed to reactRTCApprovalByDecision");
}

/*
* for InternalError
*/
template <>
void BlindSpotModule::setRTCStatusByDecision(
[[maybe_unused]] const InternalError & decision,
[[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
return;
}

template <>
void BlindSpotModule::reactRTCApprovalByDecision(
[[maybe_unused]] const InternalError & decision,
[[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
[[maybe_unused]] StopReason * stop_reason)
{
return;
}

/*
* For OverPassJudge
*/
template <>
void BlindSpotModule::setRTCStatusByDecision(
[[maybe_unused]] const OverPassJudge & decision,
[[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
return;
}

template <>
void BlindSpotModule::reactRTCApprovalByDecision(
[[maybe_unused]] const OverPassJudge & decision,
[[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
[[maybe_unused]] StopReason * stop_reason)
{
return;
}

/*
* for Unsafe
*/
template <>
void BlindSpotModule::setRTCStatusByDecision(
const Unsafe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
setSafe(false);
const auto & current_pose = planner_data_->current_odometry->pose;
setDistance(
motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx));
return;
}

template <>
void BlindSpotModule::reactRTCApprovalByDecision(
const Unsafe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
StopReason * stop_reason)
{
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);
}
return;
}

Check warning on line 116 in planning/behavior_velocity_blind_spot_module/src/decisions.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Code Duplication

The module contains 2 functions with similar structure: BlindSpotModule::reactRTCApprovalByDecision,BlindSpotModule::reactRTCApprovalByDecision. Avoid duplicated, aka copy-pasted, code inside the module. More duplication lowers the code health.

/*
* for Safe
*/
template <>
void BlindSpotModule::setRTCStatusByDecision(
const Safe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
setSafe(true);
const auto & current_pose = planner_data_->current_odometry->pose;
setDistance(
motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx));
return;
}

template <>
void BlindSpotModule::reactRTCApprovalByDecision(
const Safe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
StopReason * stop_reason)
{
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);
}
return;
}

} // namespace behavior_velocity_planner
11 changes: 7 additions & 4 deletions planning/behavior_velocity_blind_spot_module/src/manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,14 +63,17 @@ void BlindSpotModuleManager::launchNewModules(
}

// Is turning lane?
const std::string turn_direction = ll.attributeOr("turn_direction", "else");
if (turn_direction != "left" && turn_direction != "right") {
const std::string turn_direction_str = ll.attributeOr("turn_direction", "else");
if (turn_direction_str != "left" && turn_direction_str != "right") {
continue;
}
const auto turn_direction = turn_direction_str == "left"
? BlindSpotModule::TurnDirection::LEFT
: BlindSpotModule::TurnDirection::RIGHT;

registerModule(std::make_shared<BlindSpotModule>(
module_id, lane_id, planner_data_, planner_param_, logger_.get_child("blind_spot_module"),
clock_));
module_id, lane_id, turn_direction, planner_data_, planner_param_,
logger_.get_child("blind_spot_module"), clock_));
generateUUID(module_id);
updateRTCStatus(
getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits<double>::lowest(),
Expand Down
Loading
Loading