Skip to content

Commit

Permalink
add planning_factor_interface to behavior_velocity_planner da, vtl, n…
Browse files Browse the repository at this point in the history
…o_stopping

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
soblin committed Jan 15, 2025
1 parent d4a3ba9 commit 2f7f175
Show file tree
Hide file tree
Showing 4 changed files with 17 additions and 19 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,6 @@ DetectionAreaModule::DetectionAreaModule(
planner_param_(planner_param),
debug_data_()
{
velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA);
}

bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
Expand Down Expand Up @@ -180,9 +179,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)

// Create StopReason
{
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_point->second,
VelocityFactor::UNKNOWN);
planning_factor_interface_->add(
path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second,
tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");
}

return true;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,6 @@ NoStoppingAreaModule::NoStoppingAreaModule(
planner_param_(planner_param),
debug_data_()
{
velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA);
state_machine_.setState(StateMachine::State::GO);
state_machine_.setMarginTime(planner_param_.state_clear_time);
}
Expand Down Expand Up @@ -144,9 +143,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path)

// Create StopReason
{
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_point->second,
VelocityFactor::UNKNOWN);
planning_factor_interface_->add(
path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
tier4_planning_msgs::msg::PlanningFactor::STOP,
tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0,
0.0 /*shift distance*/, "");
}

} else if (state_machine_.getState() == StateMachine::State::GO) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,6 @@ VirtualTrafficLightModule::VirtualTrafficLightModule(
lane_(lane),
planner_param_(planner_param)
{
velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT);

// Get map data
const auto instrument = reg_elem_.getVirtualTrafficLight();
const auto instrument_bottom_line = toAutowarePoints(instrument);
Expand Down Expand Up @@ -420,9 +418,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine(
}

// Set StopReason
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN,
command_.type);
planning_factor_interface_->add(
path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");

// Set data for visualization
module_data_.stop_head_pose_at_stop_line =
Expand Down Expand Up @@ -453,8 +452,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine(
}

// Set StopReason
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
planning_factor_interface_->add(
path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");

// Set data for visualization
module_data_.stop_head_pose_at_end_line =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -113,10 +113,6 @@ class VirtualTrafficLightModule : public SceneModuleInterface

void updateInfrastructureCommand();

void setVelocityFactor(
const geometry_msgs::msg::Pose & stop_pose,
autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor);

std::optional<size_t> getPathIndexOfFirstEndLine();

bool isBeforeStartLine(const size_t end_line_idx);
Expand Down

0 comments on commit 2f7f175

Please sign in to comment.