feat: add publisher for detailed stop reason

common/autoware_ad_api_msgs/CMakeLists.txt

@@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  motion/msg/MotionFactor.msg
+  motion/msg/MotionFactorArray.msg
   common/msg/ResponseStatus.msg
   interface/srv/InterfaceVersion.srv
   DEPENDENCIES

common/autoware_ad_api_msgs/motion/msg/MotionFactor.msg

@@ -0,0 +1,27 @@
+# constants for reason
+uint16 INTERSECTION=1
+uint16 MERGE_FROM_PRIVATE_ROAD=2
+uint16 CROSSWALK=3
+uint16 WALKWAY=4
+uint16 STOP_LINE=5
+uint16 DETECTION_AREA=6
+uint16 NO_STOPPING_AREA=7
+uint16 TRAFFIC_LIGHT=8
+uint16 OBSTACLE_STOP=9
+uint16 SURROUND_OBSTACLE_CHECK=10
+uint16 BLIND_SPOT=11
+uint16 BLOCKED_BY_OBSTACLE=12
+uint16 STOPPING_LANE_CHANGE=13
+uint16 REMOTE_EMERGENCY_STOP=14
+uint16 VIRTUAL_TRAFFIC_LIGHT=15
+
+# constants for status
+uint16 STOP_FALSE=1
+uint16 STOP_TRUE=2
+
+# variables
+geometry_msgs/Pose pose
+float32 distance
+uint16 reason
+uint16 status
+string detail

common/autoware_ad_api_msgs/motion/msg/MotionFactorArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+autoware_ad_api_msgs/MotionFactor[] motion_factors

planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp

@@ -117,7 +117,8 @@ class BlindSpotModule : public SceneModuleInterface
    */
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;

planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp

@@ -59,7 +59,8 @@ class CrosswalkModule : public SceneModuleInterface
 
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;

planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp

@@ -48,7 +48,8 @@ class WalkwayModule : public SceneModuleInterface
 
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;

planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp

@@ -68,7 +68,8 @@ class DetectionAreaModule : public SceneModuleInterface
 
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -136,7 +136,8 @@ class IntersectionModule : public SceneModuleInterface
    */
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;

planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp

@@ -118,7 +118,8 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
    */
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;

planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp

@@ -85,7 +85,8 @@ class NoStoppingAreaModule : public SceneModuleInterface
 
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp

@@ -54,7 +54,8 @@ class OcclusionSpotModule : public SceneModuleInterface
    */
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
 

planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp

@@ -47,7 +47,8 @@ class RunOutModule : public SceneModuleInterface
 
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;

planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp

@@ -19,6 +19,10 @@
 
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+// #include <tier4_planning_msgs/msg/motion_factor.hpp>
+// #include <tier4_planning_msgs/msg/motion_factor_array.hpp>
+#include <autoware_ad_api_msgs/msg/motion_factor.hpp>
+#include <autoware_ad_api_msgs/msg/motion_factor_array.hpp>
 #include <tier4_planning_msgs/msg/stop_reason.hpp>
 #include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 #include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
@@ -46,7 +50,8 @@ class SceneModuleInterface
 
   virtual bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) = 0;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) = 0;
   virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0;
   virtual visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() = 0;
 
@@ -93,6 +98,8 @@ class SceneModuleManagerInterface
     pub_infrastructure_commands_ =
       node.create_publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>(
         "~/output/infrastructure_commands", 20);
+    pub_motion_factor_ = node.create_publisher<autoware_ad_api_msgs::msg::MotionFactorArray>(
+      "~/output/motion_factors", 20);
   }
 
   virtual ~SceneModuleManagerInterface() = default;
@@ -116,21 +123,27 @@ class SceneModuleManagerInterface
     visualization_msgs::msg::MarkerArray debug_marker_array;
     visualization_msgs::msg::MarkerArray virtual_wall_marker_array;
     tier4_planning_msgs::msg::StopReasonArray stop_reason_array;
+    autoware_ad_api_msgs::msg::MotionFactorArray motion_factor_array;
     stop_reason_array.header.frame_id = "map";
     stop_reason_array.header.stamp = clock_->now();
+    motion_factor_array.header.frame_id = "map";
+    motion_factor_array.header.stamp = clock_->now();
 
     tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array;
     infrastructure_command_array.stamp = clock_->now();
 
     first_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
     for (const auto & scene_module : scene_modules_) {
       tier4_planning_msgs::msg::StopReason stop_reason;
+      autoware_ad_api_msgs::msg::MotionFactor motion_factor;
       scene_module->setPlannerData(planner_data_);
-      scene_module->modifyPathVelocity(path, &stop_reason);
-
+      scene_module->modifyPathVelocity(path, &stop_reason, &motion_factor);
       if (stop_reason.reason != "") {
         stop_reason_array.stop_reasons.emplace_back(stop_reason);
       }
+      if (motion_factor.reason != 0) {
+        motion_factor_array.motion_factors.emplace_back(motion_factor);
+      }
 
       if (const auto command = scene_module->getInfrastructureCommand()) {
         infrastructure_command_array.commands.push_back(*command);
@@ -152,6 +165,9 @@ class SceneModuleManagerInterface
     if (!stop_reason_array.stop_reasons.empty()) {
       pub_stop_reason_->publish(stop_reason_array);
     }
+    if (!motion_factor_array.motion_factors.empty()) {
+      pub_motion_factor_->publish(motion_factor_array);
+    }
     pub_infrastructure_commands_->publish(infrastructure_command_array);
     pub_debug_->publish(debug_marker_array);
     pub_virtual_wall_->publish(virtual_wall_marker_array);
@@ -213,6 +229,7 @@ class SceneModuleManagerInterface
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_virtual_wall_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_;
   rclcpp::Publisher<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr pub_stop_reason_;
+  rclcpp::Publisher<autoware_ad_api_msgs::msg::MotionFactorArray>::SharedPtr pub_motion_factor_;
   rclcpp::Publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>::SharedPtr
     pub_infrastructure_commands_;
 };

planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp

@@ -84,7 +84,8 @@ class StopLineModule : public SceneModuleInterface
 
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
@@ -109,7 +110,8 @@ class StopLineModule : public SceneModuleInterface
   autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const StopLineModule::SegmentIndexWithPose & insert_index_with_pose,
-    tier4_planning_msgs::msg::StopReason * stop_reason);
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor);
 
   lanelet::ConstLineString3d stop_line_;
   int64_t lane_id_;

planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp

@@ -72,7 +72,8 @@ class TrafficLightModule : public SceneModuleInterface
 
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
@@ -98,7 +99,8 @@ class TrafficLightModule : public SceneModuleInterface
   autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & input,
     const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point,
-    tier4_planning_msgs::msg::StopReason * stop_reason);
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor);
 
   bool isPassthrough(const double & signed_arc_length) const;
 

planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp

@@ -80,7 +80,8 @@ class VirtualTrafficLightModule : public SceneModuleInterface
 
   bool modifyPathVelocity(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason) override;
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor) override;
 
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
@@ -100,6 +101,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface
   void setStopReason(
     const geometry_msgs::msg::Pose & stop_pose, tier4_planning_msgs::msg::StopReason * stop_reason);
 
+  void setMotionFactor(
+    const geometry_msgs::msg::Pose & stop_pose,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor);
+
   bool isBeforeStartLine();
 
   bool isBeforeStopLine();
@@ -116,11 +121,13 @@ class VirtualTrafficLightModule : public SceneModuleInterface
 
   void insertStopVelocityAtStopLine(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason);
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor);
 
   void insertStopVelocityAtEndLine(
     autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-    tier4_planning_msgs::msg::StopReason * stop_reason);
+    tier4_planning_msgs::msg::StopReason * stop_reason,
+    autoware_ad_api_msgs::msg::MotionFactor * motion_factor);
 };
 }  // namespace behavior_velocity_planner
 #endif  // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -20,6 +20,7 @@
 #include <tier4_autoware_utils/trajectory/trajectory.hpp>
 #include <utilization/boost_geometry_helper.hpp>
 
+#include <autoware_ad_api_msgs/msg/motion_factor.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path.hpp>
@@ -280,6 +281,7 @@ double findReachTime(
   const double t_min, const double t_max);
 
 tier4_planning_msgs::msg::StopReason initializeStopReason(const std::string & stop_reason);
+autoware_ad_api_msgs::msg::MotionFactor initializeMotionFactor(const uint16_t stop_reason);
 
 void appendStopReason(
   const tier4_planning_msgs::msg::StopFactor stop_factor,

planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml

@@ -49,6 +49,7 @@
     <remap from="~/input/occupancy_grid" to="/sensing/lidar/occupancy_grid"/>
     <remap from="~/output/path" to="path"/>
     <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
+    <remap from="~/output/stop_reason2" to="/planning/scenario_planning/status/stop_reason2"/>
     <remap from="~/output/infrastructure_commands" to="/planning/scenario_planning/status/infrastructure_commands"/>
     <remap from="~/output/traffic_signal" to="debug/traffic_signal"/>
 

planning/behavior_velocity_planner/package.xml

@@ -12,6 +12,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_ad_api_msgs</depend>
   <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>

planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp

@@ -59,11 +59,14 @@ BlindSpotModule::BlindSpotModule(
 
 bool BlindSpotModule::modifyPathVelocity(
   autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-  tier4_planning_msgs::msg::StopReason * stop_reason)
+  tier4_planning_msgs::msg::StopReason * stop_reason,
+  autoware_ad_api_msgs::msg::MotionFactor * motion_factor)
 {
   debug_data_ = DebugData();
   *stop_reason =
     planning_utils::initializeStopReason(tier4_planning_msgs::msg::StopReason::BLIND_SPOT);
+  *motion_factor =
+    planning_utils::initializeMotionFactor(autoware_ad_api_msgs::msg::MotionFactor::BLIND_SPOT);
 
   const auto input_path = *path;
   debug_data_.path_raw = input_path;
@@ -145,6 +148,11 @@ bool BlindSpotModule::modifyPathVelocity(
     stop_factor.stop_pose = debug_data_.stop_point_pose;
     stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
     planning_utils::appendStopReason(stop_factor, stop_reason);
+    motion_factor->status = autoware_ad_api_msgs::msg::MotionFactor::STOP_TRUE;
+    motion_factor->pose = debug_data_.stop_point_pose;
+    // motion_factor->stop_factor_points =
+    //   planning_utils::toRosPoints(debug_data_.conflicting_targets);
+
   } else {
     *path = input_path;  // reset path
   }

planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp

@@ -40,13 +40,16 @@ CrosswalkModule::CrosswalkModule(
 
 bool CrosswalkModule::modifyPathVelocity(
   autoware_auto_planning_msgs::msg::PathWithLaneId * path,
-  tier4_planning_msgs::msg::StopReason * stop_reason)
+  tier4_planning_msgs::msg::StopReason * stop_reason,
+  autoware_ad_api_msgs::msg::MotionFactor * motion_factor)
 {
   debug_data_ = DebugData();
   debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   first_stop_path_point_index_ = static_cast<int>(path->points.size()) - 1;
   *stop_reason =
     planning_utils::initializeStopReason(tier4_planning_msgs::msg::StopReason::CROSSWALK);
+  *motion_factor =
+    planning_utils::initializeMotionFactor(autoware_ad_api_msgs::msg::MotionFactor::CROSSWALK);
 
   const auto input = *path;
 
@@ -91,6 +94,9 @@ bool CrosswalkModule::modifyPathVelocity(
       stop_factor.stop_pose = debug_data_.first_stop_pose;
       stop_factor.stop_factor_points = debug_data_.stop_factor_points;
       planning_utils::appendStopReason(stop_factor, stop_reason);
+      motion_factor->status = autoware_ad_api_msgs::msg::MotionFactor::STOP_TRUE;
+      motion_factor->pose = debug_data_.first_stop_pose;
+      // motion_factor->stop_factor_points = debug_data_.stop_factor_points;
     }
   }
   return true;