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

feat!: remove planning factor type #5793

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 @@ -173,10 +173,7 @@
switch (e.status) {
case SteeringFactor::APPROACHING:
label->setText("APPROACHING");
break;

Check notice on line 176 in common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method

VelocitySteeringFactorsPanel::onSteeringFactors decreases in cyclomatic complexity from 11 to 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
case SteeringFactor::TRYING:
label->setText("TRYING");
break;
case SteeringFactor::TURNING:
label->setText("TURNING");
break;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -490,16 +490,10 @@ void BehaviorPathPlannerNode::publish_steering_factor(

const auto [intersection_pose, intersection_distance] =
planner_data->turn_signal_decider.getIntersectionPoseAndDistance();
const uint16_t steering_factor_state = std::invoke([&intersection_flag]() {
if (intersection_flag) {
return SteeringFactor::TURNING;
}
return SteeringFactor::TRYING;
});

steering_factor_interface_ptr_->updateSteeringFactor(
{intersection_pose, intersection_pose}, {intersection_distance, intersection_distance},
PlanningBehavior::INTERSECTION, steering_factor_direction, steering_factor_state, "");
PlanningBehavior::INTERSECTION, steering_factor_direction, SteeringFactor::TURNING, "");
} else {
steering_factor_interface_ptr_->clearSteeringFactors();
}
Expand Down
86 changes: 0 additions & 86 deletions system/default_ad_api/src/planning.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check notice on line 1 in system/default_ad_api/src/planning.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Overall Code Complexity

The mean cyclomatic complexity in this module is no longer above the threshold
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -62,83 +62,7 @@
}
return message;
}

Check notice on line 65 in system/default_ad_api/src/planning.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Method

convert_velocity_behavior is no longer above the threshold for cyclomatic complexity. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
uint16_t convert_velocity_behavior(const std::string & behavior)
{
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
using autoware_adapi_v1_msgs::msg::VelocityFactor;

if (behavior == PlanningBehavior::AVOIDANCE) {
return VelocityFactor::AVOIDANCE;
}
if (behavior == PlanningBehavior::CROSSWALK) {
return VelocityFactor::CROSSWALK;
}
if (behavior == PlanningBehavior::INTERSECTION) {
return VelocityFactor::INTERSECTION;
}
if (behavior == PlanningBehavior::LANE_CHANGE) {
return VelocityFactor::LANE_CHANGE;
}
if (behavior == PlanningBehavior::MERGE) {
return VelocityFactor::MERGE;
}
if (behavior == PlanningBehavior::NO_DRIVABLE_LANE) {
return VelocityFactor::NO_DRIVABLE_LANE;
}
if (behavior == PlanningBehavior::NO_STOPPING_AREA) {
return VelocityFactor::NO_STOPPING_AREA;
}
if (behavior == PlanningBehavior::REAR_CHECK) {
return VelocityFactor::REAR_CHECK;
}
if (behavior == PlanningBehavior::ROUTE_OBSTACLE) {
return VelocityFactor::ROUTE_OBSTACLE;
}
if (behavior == PlanningBehavior::SIDEWALK) {
return VelocityFactor::SIDEWALK;
}
if (behavior == PlanningBehavior::STOP_SIGN) {
return VelocityFactor::STOP_SIGN;
}
if (behavior == PlanningBehavior::SURROUNDING_OBSTACLE) {
return VelocityFactor::SURROUNDING_OBSTACLE;
}
if (behavior == PlanningBehavior::TRAFFIC_SIGNAL) {
return VelocityFactor::TRAFFIC_SIGNAL;
}
if (behavior == PlanningBehavior::USER_DEFINED_DETECTION_AREA) {
return VelocityFactor::USER_DEFINED_DETECTION_AREA;
}
if (behavior == PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT) {
return VelocityFactor::V2I_GATE_CONTROL_ENTER;
}
return VelocityFactor::UNKNOWN;
}

uint16_t convert_steering_behavior(const std::string & behavior)
{
using autoware_adapi_v1_msgs::msg::PlanningBehavior;
using autoware_adapi_v1_msgs::msg::SteeringFactor;

if (behavior == PlanningBehavior::AVOIDANCE) {
return SteeringFactor::AVOIDANCE_PATH_CHANGE;
}
if (behavior == PlanningBehavior::GOAL_PLANNER) {
return SteeringFactor::GOAL_PLANNER;
}
if (behavior == PlanningBehavior::INTERSECTION) {
return SteeringFactor::INTERSECTION;
}
if (behavior == PlanningBehavior::LANE_CHANGE) {
return SteeringFactor::LANE_CHANGE;
}
if (behavior == PlanningBehavior::START_PLANNER) {
return SteeringFactor::START_PLANNER;
}
return SteeringFactor::UNKNOWN;
}

PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning", options)
{
// TODO(Takagi, Isamu): remove default value
Expand Down Expand Up @@ -202,17 +126,7 @@
using autoware_adapi_v1_msgs::msg::VelocityFactor;
auto velocity = merge_factors<VelocityFactorArray>(now(), velocity_factors_);
auto steering = merge_factors<SteeringFactorArray>(now(), steering_factors_);

Check notice on line 129 in system/default_ad_api/src/planning.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method

PlanningNode::on_timer decreases in cyclomatic complexity from 12 to 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
// Set velocity factor type for compatibility.
for (auto & factor : velocity.factors) {
factor.type = convert_velocity_behavior(factor.behavior);
}

// Set steering factor type for compatibility.
for (auto & factor : steering.factors) {
factor.type = convert_steering_behavior(factor.behavior);
}

// Set the distance if it is nan.
if (trajectory_ && kinematic_state_) {
for (auto & factor : velocity.factors) {
Expand Down
Loading