feat: change planning factor behavior constants

common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp

@@ -107,62 +107,9 @@
   for (std::size_t i = 0; i < msg->factors.size(); i++) {
     const auto & e = msg->factors.at(i);
 
-    // type
+    // behavior
     {
-      auto label = new QLabel();
-      switch (e.type) {
-        case VelocityFactor::SURROUNDING_OBSTACLE:
-          label->setText("SURROUNDING_OBSTACLE");
-          break;
-        case VelocityFactor::ROUTE_OBSTACLE:
-          label->setText("ROUTE_OBSTACLE");
-          break;
-        case VelocityFactor::INTERSECTION:
-          label->setText("INTERSECTION");
-          break;
-        case VelocityFactor::CROSSWALK:
-          label->setText("CROSSWALK");
-          break;
-        case VelocityFactor::REAR_CHECK:
-          label->setText("REAR_CHECK");
-          break;
-        case VelocityFactor::USER_DEFINED_DETECTION_AREA:
-          label->setText("USER_DEFINED_DETECTION_AREA");
-          break;
-        case VelocityFactor::NO_STOPPING_AREA:
-          label->setText("NO_STOPPING_AREA");
-          break;
-        case VelocityFactor::STOP_SIGN:
-          label->setText("STOP_SIGN");
-          break;
-        case VelocityFactor::TRAFFIC_SIGNAL:
-          label->setText("TRAFFIC_SIGNAL");
-          break;
-        case VelocityFactor::V2I_GATE_CONTROL_ENTER:
-          label->setText("V2I_GATE_CONTROL_ENTER");
-          break;
-        case VelocityFactor::V2I_GATE_CONTROL_LEAVE:
-          label->setText("V2I_GATE_CONTROL_LEAVE");
-          break;
-        case VelocityFactor::MERGE:
-          label->setText("MERGE");
-          break;
-        case VelocityFactor::SIDEWALK:
-          label->setText("SIDEWALK");
-          break;
-        case VelocityFactor::LANE_CHANGE:
-          label->setText("LANE_CHANGE");
-          break;
-        case VelocityFactor::AVOIDANCE:
-          label->setText("AVOIDANCE");
-          break;
-        case VelocityFactor::EMERGENCY_STOP_OPERATION:
-          label->setText("EMERGENCY_STOP_OPERATION");
-          break;
-        default:
-          label->setText("UNKNOWN");
-          break;
-      }
+      auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str());
       label->setAlignment(Qt::AlignCenter);
       velocity_factors_table_->setCellWidget(i, 0, label);
     }
@@ -213,38 +160,9 @@
   for (std::size_t i = 0; i < msg->factors.size(); i++) {
     const auto & e = msg->factors.at(i);
 
-    // type
+    // behavior
     {
-      auto label = new QLabel();
-      switch (e.type) {
-        case SteeringFactor::INTERSECTION:
-          label->setText("INTERSECTION");
-          break;
-        case SteeringFactor::LANE_CHANGE:
-          label->setText("LANE_CHANGE");
-          break;
-        case SteeringFactor::AVOIDANCE_PATH_CHANGE:
-          label->setText("AVOIDANCE_PATH_CHANGE");
-          break;
-        case SteeringFactor::AVOIDANCE_PATH_RETURN:
-          label->setText("AVOIDANCE_PATH_RETURN");
-          break;
-        case SteeringFactor::STATION:
-          label->setText("STATION");
-          break;
-        case SteeringFactor::START_PLANNER:
-          label->setText("START_PLANNER");
-          break;
-        case SteeringFactor::GOAL_PLANNER:
-          label->setText("GOAL_PLANNER");
-          break;
-        case SteeringFactor::EMERGENCY_OPERATION:
-          label->setText("EMERGENCY_OPERATION");
-          break;
-        default:
-          label->setText("UNKNOWN");
-          break;
-      }
+      auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str());
       label->setAlignment(Qt::AlignCenter);
       steering_factors_table_->setCellWidget(i, 0, label);
     }

common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp

@@ -26,6 +26,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rviz_common/panel.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
 #include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 
@@ -35,6 +36,7 @@ namespace rviz_plugins
 {
 class VelocitySteeringFactorsPanel : public rviz_common::Panel
 {
+  using PlanningBehavior = autoware_adapi_v1_msgs::msg::PlanningBehavior;
   using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray;
   using VelocityFactor = autoware_adapi_v1_msgs::msg::VelocityFactor;
   using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -123,7 +123,7 @@
       if (finish_distance > -1.0e-03) {
         steering_factor_interface_ptr_->updateSteeringFactor(
           {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
-          SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::LEFT, SteeringFactor::TURNING, "");
+          PlanningBehavior::AVOIDANCE, SteeringFactor::LEFT, SteeringFactor::TURNING, "");
       }
     }
 
@@ -137,8 +137,7 @@
       if (finish_distance > -1.0e-03) {
         steering_factor_interface_ptr_->updateSteeringFactor(
           {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},
-          SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::RIGHT, SteeringFactor::TURNING,
-          "");
+          PlanningBehavior::AVOIDANCE, SteeringFactor::RIGHT, SteeringFactor::TURNING, "");
       }
     }
   }
@@ -387,7 +386,7 @@
       removeRTCStatus();
     }
 
     generator_.reset();
     path_shifter_.setShiftLines(ShiftLineArray{});
   }
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -34,6 +34,7 @@
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
 #include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
@@ -55,6 +56,7 @@
 
 namespace behavior_path_planner
 {
+using autoware_adapi_v1_msgs::msg::PlanningBehavior;
 using autoware_adapi_v1_msgs::msg::SteeringFactor;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using objects_of_interest_marker_interface::ColorName;

planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp

@@ -35,8 +35,9 @@ class SteeringFactorInterface
   SteeringFactorInterface(rclcpp::Node * node, const std::string & name);
   void publishSteeringFactor(const rclcpp::Time & stamp);
   void updateSteeringFactor(
-    const std::array<Pose, 2> & pose, const std::array<double, 2> distance, const uint16_t type,
-    const uint16_t direction, const uint16_t status, const std::string detail);
+    const std::array<Pose, 2> & poses, const std::array<double, 2> distances,
+    const std::string & behavior, const uint16_t direction, const uint16_t status,
+    const std::string detail);
   void clearSteeringFactors();
 
 private:

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -647,7 +647,7 @@ void BehaviorPathPlannerNode::publish_steering_factor(
 
     steering_factor_interface_ptr_->updateSteeringFactor(
       {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance},
-      SteeringFactor::INTERSECTION, steering_factor_direction, steering_factor_state, "");
+      PlanningBehavior::INTERSECTION, steering_factor_direction, steering_factor_state, "");
   } else {
     steering_factor_interface_ptr_->clearSteeringFactors();
   }

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 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.
@@ -178,7 +178,7 @@
 
   // If the vehicle is on the shift line, keep RUNNING.
   {
     const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
     const auto within = [](const auto & line, const size_t idx) {
       return line.start_idx < idx && idx < line.end_idx;
     };
@@ -409,7 +409,7 @@
         getLogger(),
         "Distance to shift start point is less than minimum prepare distance. The distance is not "
         "enough.");
       return false;
     }
   }
 
@@ -518,8 +518,8 @@
     if (o.avoid_required && enough_space) {
       data.avoid_required = true;
       data.stop_target_object = o;
       data.to_stop_line = o.to_stop_line;
       break;
     }
   }
 
@@ -531,9 +531,9 @@
   if (isOutputPathLocked()) {
     data.safe_shift_line.clear();
     data.candidate_path = helper_->getPreviousSplineShiftPath();
     RCLCPP_DEBUG_THROTTLE(
       getLogger(), *clock_, 500, "this module is locked now. keep current path.");
     return;
   }
 
   /**
@@ -550,9 +550,9 @@
    * the shift line is unsafe.
    */
   if (!parameters_->enable_yield_maneuver) {
     data.yield_required = false;
     data.safe_shift_line = data.new_shift_line;
     return;
   }
 
   /**
@@ -561,29 +561,29 @@
    * if the avoidance has already been initiated.
    */
   if (!can_yield_maneuver) {
     data.safe = true;  // overwrite safety judge.
     data.yield_required = false;
     data.safe_shift_line = data.new_shift_line;
     RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. but could not transit yield status.");
     return;
   }
 
   /**
    * Transit yield maneuver. Clear shift lines and output yield path.
    */
   {
     data.yield_required = true;
     data.safe_shift_line = data.new_shift_line;
   }
 
   /**
    * Even if data.avoid_required is false, the module cancels registered shift point when the
    * approved avoidance path is not safe.
    */
   const auto approved_path_exist = !path_shifter_.getShiftLines().empty();
   if (approved_path_exist) {
     RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe. canceling approved path...");
     return;
   }
 
   /**
@@ -591,7 +591,7 @@
    * stops in front of the front object with the necessary distance to avoid the object.
    */
   {
     RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe. transit yield maneuver...");
   }
 }
 
@@ -610,26 +610,26 @@
     return;
   }
 
   const auto o_front = data.stop_target_object.get();
   const auto object_type = utils::getHighestProbLabel(o_front.object.classification);
   const auto object_parameter = parameters_->object_parameters.at(object_type);
   const auto & additional_buffer_longitudinal =
     object_parameter.use_conservative_buffer_longitudinal
       ? planner_data_->parameters.base_link2front
       : 0.0;
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
   const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor +
                                 object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
 
   const auto variable = helper_->getSharpAvoidanceDistance(
     helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
   const auto constant = helper_->getNominalPrepareDistance() +
                         object_parameter.safety_buffer_longitudinal +
                         additional_buffer_longitudinal;
   const auto total_avoid_distance = variable + constant;
 
   dead_pose_ = calcLongitudinalOffsetPose(
     data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance);
 
   if (!dead_pose_) {
@@ -637,7 +637,7 @@
   }
 }
 
 AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const
 {
   if (data.yield_required) {
     return AvoidanceState::YIELD;
@@ -652,48 +652,48 @@
   }
 
   if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) {
     return AvoidanceState::AVOID_PATH_READY;
   }
 
   return AvoidanceState::AVOID_EXECUTE;
 }
 
 void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path)
 {
   if (parameters_->disable_path_update) {
     return;
   }
 
   insertPrepareVelocity(path);
 
   switch (data.state) {
     case AvoidanceState::NOT_AVOID: {
       break;
     }
     case AvoidanceState::YIELD: {
       insertYieldVelocity(path);
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
       break;
     }
     case AvoidanceState::AVOID_PATH_NOT_READY: {
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
       break;
     }
     case AvoidanceState::AVOID_PATH_READY: {
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
       break;
     }
     case AvoidanceState::AVOID_EXECUTE: {
       insertStopPoint(isBestEffort(parameters_->policy_deceleration), path);
       break;
     }
     default:
       throw std::domain_error("invalid behavior");
   }
 
   insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path);
 
   setStopReason(StopReason::AVOIDANCE, path.path);
 }
 
 bool AvoidanceModule::isSafePath(
@@ -773,14 +773,14 @@
       if (!utils::path_safety_checker::checkCollision(
             shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
             hysteresis_factor, current_debug_data.second)) {
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           debug.collision_check, current_debug_data, false);
 
         safe_count_ = 0;
         return false;
       }
     }
     utils::path_safety_checker::updateCollisionCheckDebugMap(
       debug.collision_check, current_debug_data, true);
   }
 
@@ -875,7 +875,7 @@
     helper_->setPreviousSplineShiftPath(spline_shift_path);
     helper_->setPreviousReferencePath(data.reference_path);
   } else {
     spline_shift_path = helper_->getPreviousSplineShiftPath();
   }
 
   // post processing
@@ -972,7 +972,7 @@
   utils::clipPathLength(
     shifted_path.path, sl_front.start_idx, std::numeric_limits<double>::max(), 0.0);
 
   output.lateral_shift = helper_->getRelativeShiftToPath(sl);
   output.start_distance_to_path_change = sl_front.start_longitudinal;
   output.finish_distance_to_path_change = sl_back.end_longitudinal;
 
@@ -982,8 +982,7 @@
   steering_factor_interface_ptr_->updateSteeringFactor(
     {sl_front.start, sl_back.end},
     {output.start_distance_to_path_change, output.finish_distance_to_path_change},
-    SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING,
-    "");
+    PlanningBehavior::AVOIDANCE, steering_factor_direction, SteeringFactor::APPROACHING, "");
 
   output.path_candidate = shifted_path.path;
   return output;
@@ -1019,9 +1018,9 @@
 
   addNewShiftLines(path_shifter_, shift_lines);
 
   generator_.setRawRegisteredShiftLine(shift_lines, avoid_data_);
 
   const auto sl = helper_->getMainShiftLine(shift_lines);
   const auto sl_front = shift_lines.front();
   const auto sl_back = shift_lines.back();
 
@@ -1273,7 +1272,7 @@
   const auto sl_back = candidates.back();
 
   output.path_candidate = data.candidate_path.path;
   output.lateral_shift = helper_->getRelativeShiftToPath(shift_line);
   output.start_distance_to_path_change = sl_front.start_longitudinal;
   output.finish_distance_to_path_change = sl_back.end_longitudinal;
 
@@ -1410,7 +1409,7 @@
     return;
   }
 
   debug_marker_ = marker_utils::avoidance_marker::createDebugMarkerArray(data, shifter, debug);
 }
 
 void AvoidanceModule::updateAvoidanceDebugData(
@@ -1456,8 +1455,8 @@
 
   const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
                             object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
   const auto variable = helper_->getMinAvoidanceDistance(
     helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
   const auto & additional_buffer_longitudinal =
     object_parameter.use_conservative_buffer_longitudinal
       ? planner_data_->parameters.base_link2front
@@ -1505,7 +1504,7 @@
   }
 
   // If the stop distance is not enough for comfortable stop, don't insert wait point.
   const auto is_comfortable_stop = helper_->getFeasibleDecelDistance(0.0) < to_stop_line;
   if (!is_comfortable_stop) {
     RCLCPP_DEBUG(getLogger(), "stop distance is not enough.");
     return;
@@ -1581,7 +1580,7 @@
 
   // If the object cannot be stopped for, calculate a "mild" deceleration distance
   // and insert a deceleration point at that distance
   const auto stop_distance = helper_->getFeasibleDecelDistance(0.0, false);
   utils::avoidance::insertDecelPoint(
     getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_);
 }
@@ -1624,7 +1623,7 @@
   }
 
   // Otherwise, consider deceleration constraints before inserting deceleration point
   const auto decel_distance = helper_->getFeasibleDecelDistance(0.0, false);
   if (stop_distance < decel_distance) {
     return;
   }
@@ -1647,7 +1646,7 @@
     return;
   }
 
   const auto decel_distance = helper_->getFeasibleDecelDistance(p->yield_velocity, false);
   utils::avoidance::insertDecelPoint(
     getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, slow_pose_);
 }

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -266,7 +266,7 @@
   if (thread_safe_data_.get_goal_candidates().empty()) {
     goal_searcher_->setPlannerData(planner_data_);
     goal_searcher_->setReferenceGoal(
       calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose()));
     thread_safe_data_.set_goal_candidates(generateGoalCandidates());
   }
 
@@ -314,7 +314,7 @@
   resetPathCandidate();
   resetPathReference();
   debug_marker_.markers.clear();
   prev_data_.reset();
   last_approval_data_.reset();
 }
 
@@ -524,7 +524,7 @@
   return false;
 }
 
 bool GoalPlannerModule::canReturnToLaneParking()
 {
   // return only before starting free space parking
   if (!isStopped()) {
@@ -553,25 +553,25 @@
   const bool is_close_to_path =
     std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance;
   if (!is_close_to_path) {
     return false;
   }
 
   return true;
 }
 
 GoalCandidates GoalPlannerModule::generateGoalCandidates() const
 {
   // calculate goal candidates
   const auto & route_handler = planner_data_->route_handler;
   if (goal_planner_utils::isAllowedGoalModification(route_handler)) {
     return goal_searcher_->search();
   }
 
   // NOTE:
   // currently since pull over is performed only when isAllowedGoalModification is true,
   // never be in the following process.
   GoalCandidate goal_candidate{};
   goal_candidate.goal_pose = route_handler->getOriginalGoalPose();
   goal_candidate.distance_from_original_goal = 0.0;
   GoalCandidates goal_candidates{};
   goal_candidates.push_back(goal_candidate);
@@ -610,7 +610,7 @@
 
   if (parameters_->use_object_recognition) {
     // Create a map of PullOverPath pointer to largest collision check margin
     auto calcLargestMargin = [&](const PullOverPath & pull_over_path) {
       // check has safe goal
       const auto goal_candidate_it = std::find_if(
         goal_candidates.begin(), goal_candidates.end(),
@@ -621,17 +621,17 @@
         return 0.0;
       }
       // calc largest margin
       std::vector<double> scale_factors{3.0, 2.0, 1.0};
       const double margin = parameters_->object_recognition_collision_check_margin;
       const auto resampled_path =
         utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5);
       for (const auto & scale_factor : scale_factors) {
         if (!checkObjectsCollision(resampled_path, margin * scale_factor)) {
           return margin * scale_factor;
         }
       }
       return 0.0;
     };
 
     // Create a map of PullOverPath pointer to largest collision check margin
     std::map<size_t, double> path_id_to_margin_map;
@@ -640,13 +640,13 @@
     }
 
     // sorts in descending order so the item with larger margin comes first
     std::stable_sort(
       sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(),
       [&path_id_to_margin_map](const PullOverPath & a, const PullOverPath & b) {
         if (std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01) {
           return false;
         }
         return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id];
       });
   }
 
@@ -665,7 +665,7 @@
   return sorted_pull_over_path_candidates;
 }
 
 std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectPullOverPath(
   const std::vector<PullOverPath> & pull_over_path_candidates,
   const GoalCandidates & goal_candidates, const double collision_check_margin) const
 {
@@ -685,19 +685,19 @@
     }
 
     const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5);
     if (
       parameters_->use_object_recognition &&
       checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) {
       continue;
     }
 
     if (
       parameters_->use_occupancy_grid_for_path_collision_check &&
       checkOccupancyGridCollision(resampled_path)) {
       continue;
     }
 
     return std::make_pair(pull_over_path, *goal_candidate_it);
   }
 
   return {};
@@ -853,7 +853,7 @@
 
   // TODO(tkhmy) add handle status TRYING
   steering_factor_interface_ptr_->updateSteeringFactor(
-    pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, "");
+    pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, "");
 }
 
 bool GoalPlannerModule::hasDecidedPath() const
@@ -950,11 +950,11 @@
     // select safe path from lane parking pull over path candidates
     // and set it to thread_safe_data_
 
     thread_safe_data_.clearPullOverPath();
 
     // update goal candidates
     goal_searcher_->setPlannerData(planner_data_);
     auto goal_candidates = thread_safe_data_.get_goal_candidates();
     goal_searcher_->update(goal_candidates);
 
     // update pull over path candidates
@@ -1029,7 +1029,7 @@
   setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath());
 }
 
 void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
 {
   if (prev_data_.found_path || !prev_data_.stop_path) {
     prev_data_.stop_path = output.path;
@@ -1037,19 +1037,19 @@
 
   // for the next loop setOutput().
   // this is used to determine whether to generate a new stop path or keep the current stop path.
   prev_data_.found_path = thread_safe_data_.foundPullOverPath();
 
   prev_data_.has_decided_path = hasDecidedPath();
 
   // This is related to safety check, so if it is disabled, return here.
   if (!parameters_->safety_check_params.enable_safety_check) {
     prev_data_.safety_status.is_safe = true;
     return;
   }
 
   // Even if the current path is safe, it will not be safe unless it stands for a certain period of
   // time. Record the time when the current path has become safe
   const auto [is_safe, current_is_safe] = isSafePath();
   if (current_is_safe) {
     if (!prev_data_.safety_status.safe_start_time) {
       prev_data_.safety_status.safe_start_time = clock_->now();
@@ -1057,7 +1057,7 @@
   } else {
     prev_data_.safety_status.safe_start_time = std::nullopt;
   }
   prev_data_.safety_status.is_safe = is_safe;
 
   // If safety check is enabled, save current path as stop_path_after_approval
   // This path is set only once after approval.
@@ -1066,7 +1066,7 @@
   }
   auto stop_path = std::make_shared<PathWithLaneId>(*output.path);
   for (auto & point : stop_path->points) {
     point.point.longitudinal_velocity_mps = 0.0;
   }
   prev_data_.stop_path_after_approval = stop_path;
 }
@@ -1124,9 +1124,9 @@
   const double pull_over_velocity = parameters_->pull_over_velocity;
 
   const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
     /*forward_only_in_route*/ false);
 
   if (current_lanes.empty()) {
     return PathWithLaneId{};
@@ -1215,9 +1215,9 @@
 
   // generate stop reference path
   const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
     /*forward_only_in_route*/ false);
 
   const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length;
   const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
@@ -1294,7 +1294,7 @@
     return false;
   }
 
   if (
     parameters_->use_object_recognition &&
     checkObjectsCollision(
       thread_safe_data_.get_pull_over_path()->getCurrentPath(),
@@ -1302,10 +1302,10 @@
     return true;
   }
 
   if (
     parameters_->use_occupancy_grid_for_path_collision_check &&
     checkOccupancyGridCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath())) {
     return true;
   }
 
   return false;
@@ -1400,16 +1400,16 @@
   return turn_signal;
 }
 
 bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const
 {
   if (!occupancy_grid_map_) {
     return false;
   }
   const bool check_out_of_range = false;
   return occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range);
 }
 
 bool GoalPlannerModule::checkObjectsCollision(
   const PathWithLaneId & path, const double collision_check_margin,
   const bool update_debug_data) const
 {
@@ -1468,8 +1468,8 @@
       tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0);
     const auto ego_polygon = tier4_autoware_utils::toFootprint(
       lateral_offset_pose,
       planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin,
       planner_data_->parameters.base_link2rear + collision_check_margin,
       planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 +
         std::abs(extra_lateral_margin));
     ego_polygons_expanded.push_back(ego_polygon);
@@ -1550,7 +1550,7 @@
     path.points, current_pose.position, ego_idx, pose.position, target_idx);
 }
 
 void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const
 {
   // decelerate before the search area start
   const auto closest_goal_candidate =
@@ -1560,7 +1560,7 @@
     -approximate_pull_over_distance_);
   auto & first_path = pull_over_path.partial_paths.front();
   if (decel_pose) {
     decelerateBeforeSearchStart(*decel_pose, first_path);
     return;
   }
 
@@ -1569,11 +1569,11 @@
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk,
     parameters_->pull_over_velocity);
   for (auto & p : first_path.points) {
     const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose);
     if (min_decel_distance && distance_from_ego < *min_decel_distance) {
       continue;
     }
     p.point.longitudinal_velocity_mps = std::min(
       p.point.longitudinal_velocity_mps, static_cast<float>(parameters_->pull_over_velocity));
   }
 }
@@ -1742,35 +1742,35 @@
   goal_planner_data_.ego_predicted_path = ego_predicted_path;
 }
 
 bool GoalPlannerModule::checkSafetyWithRSS(
   const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
   const std::vector<ExtendedPredictedObject> & objects, const double hysteresis_factor) const
 {
   // Check for collisions with each predicted path of the object
   const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) {
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(object);
 
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, objects_filtering_params_->check_all_predicted_path);
 
     return std::any_of(
       obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) {
         const bool has_collision = !utils::path_safety_checker::checkCollision(
           planned_path, ego_predicted_path, object, obj_path, planner_data_->parameters,
           safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second);
 
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           goal_planner_data_.collision_check, current_debug_data, !has_collision);
 
         return has_collision;
       });
   });
 
   return is_safe;
 }
 
 std::pair<bool, bool> GoalPlannerModule::isSafePath() const
 {
   const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
@@ -1780,7 +1780,7 @@
   const auto & dynamic_object = planner_data_->dynamic_object;
   const auto & route_handler = planner_data_->route_handler;
   const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
     /*forward_only_in_route*/ false);
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
@@ -1819,17 +1819,17 @@
   const double hysteresis_factor =
     prev_data_.safety_status.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate;
 
   const bool current_is_safe = std::invoke([&]() {
     if (parameters_->safety_check_params.method == "RSS") {
       return checkSafetyWithRSS(
         pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane,
         hysteresis_factor);
     } else if (parameters_->safety_check_params.method == "integral_predicted_polygon") {
       return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon(
         ego_predicted_path, vehicle_info_, target_objects_on_lane.on_current_lane,
         objects_filtering_params_->check_all_predicted_path,
         parameters_->safety_check_params.integral_predicted_polygon_params,
         goal_planner_data_.collision_check);
     }
     RCLCPP_ERROR(
       getLogger(), " [pull_over] invalid safety check method: %s",
@@ -1851,15 +1851,15 @@
    *　　 0 =========================-------==========-- t
    */
   if (current_is_safe) {
     if (
       prev_data_.safety_status.safe_start_time &&
       (clock_->now() - prev_data_.safety_status.safe_start_time.value()).seconds() >
         parameters_->safety_check_params.keep_unsafe_time) {
       return {true /*is_safe*/, true /*current_is_safe*/};
     }
   }
 
   return {false /*is_safe*/, current_is_safe};
 }
 
 void GoalPlannerModule::setDebugData()
@@ -1922,7 +1922,7 @@
       "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST,
       tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0),
       tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999));
     const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
     for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) {
       for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) {
         const auto & current_point = ego_polygon.outer().at(ep_idx);
@@ -1968,13 +1968,13 @@
     // visualize safety status maker
     {
       visualization_msgs::msg::MarkerArray marker_array{};
       const auto color = prev_data_.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99)
                                                           : createMarkerColor(1.0, 0.0, 0.0, 0.99);
       auto marker = createDefaultMarker(
         header.frame_id, header.stamp, "safety_status", 0,
         visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color);
 
       marker.pose = planner_data_->self_odometry->pose.pose;
       marker.text += "is_safe: " + std::to_string(prev_data_.safety_status.is_safe) + "\n";
       if (prev_data_.safety_status.safe_start_time) {
         const double elapsed_time_from_safe_start =
@@ -1984,7 +1984,7 @@
       }
       marker_array.markers.push_back(marker);
       add(marker_array);
     }
   }
 
   // Visualize planner type text
@@ -2045,7 +2045,7 @@
 bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const
 {
   const auto & route_handler = planner_data_->route_handler;
   const Pose goal_pose = route_handler->getOriginalGoalPose();
 
   const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *(route_handler), left_side_parking_, parameters_->backward_goal_search_length,

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -202,7 +202,7 @@
 
   for (const auto & [uuid, data] : module_type_->getAfterApprovalDebugData()) {
     const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
     setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
   }
 
   updateSteeringFactorPtr(output);
@@ -230,7 +230,7 @@
 
   for (const auto & [uuid, data] : module_type_->getDebugData()) {
     const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
     setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
   }
 
   // change turn signal when the vehicle reaches at the end of the path for waiting lane change
@@ -388,7 +388,7 @@
   // TODO(tkhmy) add handle status TRYING
   steering_factor_interface_ptr_->updateSteeringFactor(
     {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end},
-    {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction,
+    {start_distance, finish_distance}, PlanningBehavior::LANE_CHANGE, steering_factor_direction,
     SteeringFactor::TURNING, "");
 }
 
@@ -405,7 +405,7 @@
   steering_factor_interface_ptr_->updateSteeringFactor(
     {selected_path.info.shift_line.start, selected_path.info.shift_line.end},
     {output.start_distance_to_path_change, output.finish_distance_to_path_change},
-    SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
+    PlanningBehavior::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
 }
 void LaneChangeInterface::acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const
 {

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -232,12 +232,12 @@
   return true;
 }
 
 bool StartPlannerModule::canTransitSuccessState()
 {
   return hasFinishedPullOut();
 }
 
 bool StartPlannerModule::canTransitIdleToRunningState()
 {
   return isActivated() && !isWaitingApproval();
 }
@@ -329,7 +329,7 @@
     // TODO(tkhmy) add handle status TRYING
     steering_factor_interface_ptr_->updateSteeringFactor(
       {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
-      {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction,
+      {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
       SteeringFactor::TURNING, "");
   } else {
     const double distance = motion_utils::calcSignedArcLength(
@@ -339,7 +339,7 @@
     // TODO(tkhmy) add handle status TRYING
     steering_factor_interface_ptr_->updateSteeringFactor(
       {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
-      SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
+      PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
   }
 
   setDebugData();
@@ -442,7 +442,7 @@
     updateRTCStatus(start_distance, finish_distance);
     steering_factor_interface_ptr_->updateSteeringFactor(
       {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
-      {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction,
+      {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
       SteeringFactor::APPROACHING, "");
   } else {
     const double distance = motion_utils::calcSignedArcLength(
@@ -451,7 +451,7 @@
     updateRTCStatus(0.0, distance);
     steering_factor_interface_ptr_->updateSteeringFactor(
       {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
-      SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
+      PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
   }
 
   setDebugData();
@@ -746,7 +746,7 @@
 
 PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
 {
   const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
 
@@ -1106,7 +1106,7 @@
       if (!utils::path_safety_checker::checkCollision(
             pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters,
             safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) {
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           start_planner_data_.collision_check, current_debug_data, false);
         is_safe_dynamic_objects = false;
         is_safe_dynamic_object = false;
@@ -1114,7 +1114,7 @@
       }
     }
     if (is_safe_dynamic_object) {
       utils::path_safety_checker::updateCollisionCheckDebugMap(
         start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object);
     }
   }
@@ -1122,7 +1122,7 @@
   return is_safe_dynamic_objects;
 }
 
 bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const
 {
   const auto & rh = planner_data_->route_handler;
 
@@ -1313,38 +1313,38 @@
   if (parameters_->verbose) {
     logPullOutStatus();
   }
 }
 
 void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const
 {
   const auto logger = getLogger();
   auto logFunc = [&logger, log_level](const char * format, ...) {
     char buffer[1024];
     va_list args;
     va_start(args, format);
     vsnprintf(buffer, sizeof(buffer), format, args);
     va_end(args);
     switch (log_level) {
       case rclcpp::Logger::Level::Debug:
         RCLCPP_DEBUG(logger, "%s", buffer);
         break;
       case rclcpp::Logger::Level::Info:
         RCLCPP_INFO(logger, "%s", buffer);
         break;
       case rclcpp::Logger::Level::Warn:
         RCLCPP_WARN(logger, "%s", buffer);
         break;
       case rclcpp::Logger::Level::Error:
         RCLCPP_ERROR(logger, "%s", buffer);
         break;
       case rclcpp::Logger::Level::Fatal:
         RCLCPP_FATAL(logger, "%s", buffer);
         break;
       default:
         RCLCPP_INFO(logger, "%s", buffer);
         break;
     }
   };
 
   logFunc("======== PullOutStatus Report ========");
 

planning/behavior_path_planner/src/steering_factor_interface.cpp

@@ -32,16 +32,17 @@ void SteeringFactorInterface::publishSteeringFactor(const rclcpp::Time & stamp)
 }
 
 void SteeringFactorInterface::updateSteeringFactor(
-  const std::array<Pose, 2> & pose, const std::array<double, 2> distance, const uint16_t type,
-  const uint16_t direction, const uint16_t status, const std::string detail)
+  const std::array<Pose, 2> & pose, const std::array<double, 2> distance,
+  const std::string & behavior, const uint16_t direction, const uint16_t status,
+  const std::string detail)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   SteeringFactor factor;
   factor.pose = pose;
   std::array<float, 2> converted_distance;
   for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast<float>(distance[i]);
   factor.distance = converted_distance;
-  factor.type = type;
+  factor.behavior = behavior;
   factor.direction = direction;
   factor.status = status;
   factor.detail = detail;

planning/behavior_velocity_blind_spot_module/src/scene.cpp

@@ -69,7 +69,7 @@
   turn_direction_(TurnDirection::INVALID),
   is_over_pass_judge_line_(false)
 {
-  velocity_factor_.init(VelocityFactor::REAR_CHECK);
+  velocity_factor_.init(PlanningBehavior::REAR_CHECK);
   planner_param_ = planner_param;
 
   const auto & assigned_lanelet =

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -184,7 +184,7 @@
   planner_param_(planner_param),
   use_regulatory_element_(reg_elem_id)
 {
-  velocity_factor_.init(VelocityFactor::CROSSWALK);
+  velocity_factor_.init(PlanningBehavior::CROSSWALK);
   passed_safety_slow_point_ = false;
 
   if (use_regulatory_element_) {

planning/behavior_velocity_detection_area_module/src/scene.cpp

@@ -47,7 +47,7 @@ DetectionAreaModule::DetectionAreaModule(
   state_(State::GO),
   planner_param_(planner_param)
 {
-  velocity_factor_.init(VelocityFactor::USER_DEFINED_DETECTION_AREA);
+  velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA);
 }
 
 LineString2d DetectionAreaModule::getStopLineGeometry2d() const