chore(intersection): publish decision status string (autowarefoundati…

…on#4456) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
kminoda · Aug 1, 2023 · 0b209f1 · 0b209f1
1 parent 9d1e688
commit 0b209f1
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 0 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -86,6 +86,8 @@ IntersectionModule::IntersectionModule(
   before_creep_state_machine_.setState(StateMachine::State::STOP);
   stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area);
   stuck_private_area_timeout_.setState(StateMachine::State::STOP);
+  decision_state_pub_ =
+    node_.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
 }
 
 void IntersectionModule::initializeRTCStatus()
@@ -648,6 +650,35 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   // calculate the
   const auto decision_result = modifyPathVelocityDetail(path, stop_reason);
 
+  std::string decision_type = "intersection" + std::to_string(module_id_) + " : ";
+  if (std::get_if<IntersectionModule::Indecisive>(&decision_result)) {
+    decision_type += "Indecisive";
+  }
+  if (std::get_if<IntersectionModule::StuckStop>(&decision_result)) {
+    decision_type += "StuckStop";
+  }
+  if (std::get_if<IntersectionModule::NonOccludedCollisionStop>(&decision_result)) {
+    decision_type += "NonOccludedCollisionStop";
+  }
+  if (std::get_if<IntersectionModule::FirstWaitBeforeOcclusion>(&decision_result)) {
+    decision_type += "FirstWaitBeforeOcclusion";
+  }
+  if (std::get_if<IntersectionModule::PeekingTowardOcclusion>(&decision_result)) {
+    decision_type += "PeekingTowardOcclusion";
+  }
+  if (std::get_if<IntersectionModule::OccludedCollisionStop>(&decision_result)) {
+    decision_type += "OccludedCollisionStop";
+  }
+  if (std::get_if<IntersectionModule::Safe>(&decision_result)) {
+    decision_type += "Safe";
+  }
+  if (std::get_if<IntersectionModule::TrafficLightArrowSolidOn>(&decision_result)) {
+    decision_type += "TrafficLightArrowSolidOn";
+  }
+  std_msgs::msg::String decision_result_msg;
+  decision_result_msg.data = decision_type;
+  decision_state_pub_->publish(decision_result_msg);
+
   prepareRTCStatus(decision_result, *path);
 
   const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -25,6 +25,7 @@
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <std_msgs/msg/string.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
@@ -272,6 +273,7 @@ class IntersectionModule : public SceneModuleInterface
   */
 
   util::DebugData debug_data_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr decision_state_pub_;
 };
 
 }  // namespace behavior_velocity_planner