Skip to content

Commit 514ad87

Browse files
authored
fix(static_obstacle_avoidance): remove velocity factor (#1740)
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 73d8679 commit 514ad87

File tree

2 files changed

+0
-18
lines changed
  • planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module

2 files changed

+0
-18
lines changed

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp

-6
Original file line numberDiff line numberDiff line change
@@ -132,9 +132,6 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
132132
}
133133

134134
if (finish_distance > -1.0e-03) {
135-
steering_factor_interface_.set(
136-
{left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
137-
SteeringFactor::LEFT, SteeringFactor::TURNING, "");
138135
planning_factor_interface_->add(
139136
start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose,
140137
PlanningFactor::SHIFT_LEFT, SafetyFactorArray{});
@@ -155,9 +152,6 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
155152
}
156153

157154
if (finish_distance > -1.0e-03) {
158-
steering_factor_interface_.set(
159-
{right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},
160-
SteeringFactor::RIGHT, SteeringFactor::TURNING, "");
161155
planning_factor_interface_->add(
162156
start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose,
163157
PlanningFactor::SHIFT_RIGHT, SafetyFactorArray{});

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

-12
Original file line numberDiff line numberDiff line change
@@ -86,8 +86,6 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule(
8686
parameters_{parameters},
8787
generator_{parameters}
8888
{
89-
steering_factor_interface_.init(PlanningBehavior::AVOIDANCE);
90-
velocity_factor_interface_.init(PlanningBehavior::AVOIDANCE);
9189
}
9290

9391
bool StaticObstacleAvoidanceModule::isExecutionRequested() const
@@ -782,8 +780,6 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior(
782780
insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path);
783781

784782
set_longitudinal_planning_factor(path.path);
785-
786-
setVelocityFactor(path.path);
787783
}
788784

789785
bool StaticObstacleAvoidanceModule::isSafePath(
@@ -1216,14 +1212,6 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const
12161212
output.start_distance_to_path_change = sl_front.start_longitudinal;
12171213
output.finish_distance_to_path_change = sl_back.end_longitudinal;
12181214

1219-
const uint16_t steering_factor_direction = std::invoke([&output]() {
1220-
return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT;
1221-
});
1222-
steering_factor_interface_.set(
1223-
{sl_front.start, sl_back.end},
1224-
{output.start_distance_to_path_change, output.finish_distance_to_path_change},
1225-
steering_factor_direction, SteeringFactor::APPROACHING, "");
1226-
12271215
const uint16_t planning_factor_direction = std::invoke([&output]() {
12281216
return output.lateral_shift > 0.0 ? PlanningFactor::SHIFT_LEFT : PlanningFactor::SHIFT_RIGHT;
12291217
});

0 commit comments

Comments
 (0)