feat: change planning factor behavior constants (#5590)

* replace module type Signed-off-by: Takagi, Isamu <[email protected]> * support compatibility Signed-off-by: Takagi, Isamu <[email protected]> --------- Signed-off-by: Takagi, Isamu <[email protected]> Co-authored-by: Hiroki OTA <[email protected]>
autowarefoundation · Nov 30, 2023 · b5417a9 · b5417a9
1 parent 118e9b6
commit b5417a9
Show file tree

Hide file tree

Showing 36 changed files with 166 additions and 148 deletions.
diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp
@@ -107,62 +107,9 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray::
   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 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray::
   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);
     }

diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp b/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;

diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -123,7 +123,7 @@ class AvoidanceModule : public SceneModuleInterface
       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 @@ class AvoidanceModule : public SceneModuleInterface
       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, "");
       }
     }
   }

diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/...havior_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;

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp b/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:

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/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();
   }

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -982,8 +982,7 @@ CandidateOutput AvoidanceModule::planCandidate() const
   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;

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -853,7 +853,7 @@ void GoalPlannerModule::updateSteeringFactor(
 
   // 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

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -388,7 +388,7 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o
   // 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 @@ void LaneChangeInterface::updateSteeringFactorPtr(
   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
 {

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -329,7 +329,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
     // 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 @@ BehaviorModuleOutput StartPlannerModule::plan()
     // 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 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     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 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     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();

diff --git a/planning/behavior_path_planner/src/steering_factor_interface.cpp b/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;

diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
@@ -69,7 +69,7 @@ BlindSpotModule::BlindSpotModule(
   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 =

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -184,7 +184,7 @@ CrosswalkModule::CrosswalkModule(
   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_) {

diff --git a/planning/behavior_velocity_detection_area_module/src/scene.cpp b/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