feat: add topic relay controller

common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp

@@ -40,6 +40,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
   const bool is_driving_forward = true);
 
+visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward);
+
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);
 

common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp

@@ -29,12 +29,13 @@ namespace autoware::motion_utils
 {
 
 /// @brief type of virtual wall associated with different marker styles and namespace
-enum VirtualWallType { stop, slowdown, deadline };
+enum VirtualWallType { stop, slowdown, deadline, pass };
 /// @brief virtual wall to be visualized in rviz
 struct VirtualWall
 {
   geometry_msgs::msg::Pose pose{};
   std::string text{};
+  std::string detail{};
   std::string ns{};
   VirtualWallType style = stop;
   double longitudinal_offset{};

common/autoware_motion_utils/src/marker/marker_helper.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.
@@ -54,7 +54,7 @@
   {
     auto marker = createDefaultMarker(
       "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
-      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+      createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0));
 
     marker.pose = vehicle_front_pose;
     marker.pose.position.z += 2.0;
@@ -85,6 +85,41 @@
 
   return marker_array;
 }
+
+inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray(
+  const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name,
+  const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id,
+  const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  // Arrow
+  {
+    auto marker = createDefaultMarker(
+      "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW,
+      createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color);
+
+    marker.pose = vehicle_front_pose;
+
+    marker_array.markers.push_back(marker);
+  }
+
+  // Factor Text
+  {
+    auto marker = createDefaultMarker(
+      "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+
+    marker.pose = vehicle_front_pose;
+    marker.pose.position.z += 2.0;
+    marker.text = module_name;
+
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+
 }  // namespace
 
 namespace autoware::motion_utils
@@ -125,6 +160,18 @@
     createMarkerColor(0.0, 1.0, 0.0, 0.5));
 }
 
+visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
+{
+  const auto pose_with_offset = autoware::universe_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
+  return createIntendedPassArrowMarkerArray(
+    pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id,
+    createMarkerColor(0.77, 0.77, 0.77, 0.5));
+}
+
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id)
 {

common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp

@@ -63,10 +63,16 @@
       case deadline:
         create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker;
         break;
+      case pass:
+        create_fn = autoware::motion_utils::createIntendedPassVirtualMarker;
+        break;
     }
+    const auto wall_text = virtual_wall.detail.empty()
+                             ? virtual_wall.text
+                             : virtual_wall.text + "(" + virtual_wall.detail + ")";
     auto markers = create_fn(
-      virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset,
-      virtual_wall.ns, virtual_wall.is_driving_forward);
+      virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns,
+      virtual_wall.is_driving_forward);
     for (auto & marker : markers.markers) {
       marker.id = static_cast<int>(marker_count_per_namespace_[marker.ns].current++);
       marker_array.markers.push_back(marker);

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_
 #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_
 
+#include "autoware/motion_utils/factor/planning_factor_interface.hpp"
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/obstacle_cruise_planner/common_structs.hpp"
 #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp"
@@ -47,15 +48,15 @@ class PlannerInterface
     rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
     const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
     const EgoNearestParam & ego_nearest_param, const std::shared_ptr<DebugData> debug_data_ptr)
-  : longitudinal_info_(longitudinal_info),
+  : planning_factor_interface_{std::make_unique<autoware::motion_utils::PlanningFactorInterface>(
+      &node, "obstacle_cruise_planner")},
+    longitudinal_info_(longitudinal_info),
     vehicle_info_(vehicle_info),
     ego_nearest_param_(ego_nearest_param),
     debug_data_ptr_(debug_data_ptr),
     slow_down_param_(SlowDownParam(node)),
     stop_param_(StopParam(node, longitudinal_info))
   {
-    velocity_factors_pub_ =
-      node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/obstacle_cruise", 1);
     stop_speed_exceeded_pub_ =
       node.create_publisher<StopSpeedExceeded>("~/output/stop_speed_exceeded", 1);
     metrics_pub_ = node.create_publisher<MetricArray>("~/metrics", 10);
@@ -101,6 +102,7 @@ class PlannerInterface
     const std::optional<geometry_msgs::msg::Pose> & stop_pose = std::nullopt,
     const std::optional<StopObstacle> & stop_obstacle = std::nullopt);
   void publishMetrics(const rclcpp::Time & current_time);
+  void publishPlanningFactors() { planning_factor_interface_->publish(); }
   void clearMetrics();
 
   void onParam(const std::vector<rclcpp::Parameter> & parameters)
@@ -128,6 +130,8 @@ class PlannerInterface
   double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; }
 
 protected:
+  std::unique_ptr<autoware::motion_utils::PlanningFactorInterface> planning_factor_interface_;
+
   // Parameters
   bool enable_debug_info_{false};
   bool enable_calculation_time_info_{false};
@@ -145,7 +149,6 @@ class PlannerInterface
     stop_watch_;
 
   // Publishers
-  rclcpp::Publisher<VelocityFactorArray>::SharedPtr velocity_factors_pub_;
   rclcpp::Publisher<StopSpeedExceeded>::SharedPtr stop_speed_exceeded_pub_;
   rclcpp::Publisher<MetricArray>::SharedPtr metrics_pub_;
 

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp

@@ -17,8 +17,6 @@
 
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
-#include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp"
-#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp"
 #include "autoware_perception_msgs/msg/predicted_object.hpp"
 #include "autoware_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
@@ -39,9 +37,6 @@
 #include <pcl/point_types.h>
 
 using autoware::vehicle_info_utils::VehicleInfo;
-using autoware_adapi_v1_msgs::msg::PlanningBehavior;
-using autoware_adapi_v1_msgs::msg::VelocityFactor;
-using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
 using autoware_perception_msgs::msg::ObjectClassification;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp

@@ -94,9 +94,6 @@ size_t getIndexWithLongitudinalOffset(
   return 0;
 }
 
-VelocityFactorArray makeVelocityFactorArray(
-  const rclcpp::Time & time, const std::string & behavior = PlanningBehavior::ROUTE_OBSTACLE,
-  const std::optional<geometry_msgs::msg::Pose> pose = std::nullopt);
 }  // namespace obstacle_cruise_utils
 
 #endif  // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_

planning/autoware_obstacle_cruise_planner/package.xml

@@ -18,7 +18,6 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>

planning/autoware_obstacle_cruise_planner/src/node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 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.
@@ -726,6 +726,7 @@
   // 8. Publish debug data
   published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp);
   planner_ptr_->publishMetrics(now());
+  planner_ptr_->publishPlanningFactors();
   publishDebugMarker();
   publishDebugInfo();
   objects_of_interest_marker_interface_.publishMarkerArray();

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 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.
@@ -335,9 +335,10 @@
       debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle);
       debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data);
 
-      velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-        planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE,
-        stop_traj_points.at(wall_idx).pose));
+      planning_factor_interface_->add(
+        stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose,
+        tier4_planning_msgs::msg::PlanningFactor::NONE,
+        tier4_planning_msgs::msg::SafetyFactorArray{});
     }
 
     // do cruise planning

planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp

@@ -182,8 +182,6 @@
                                   : std::abs(vehicle_info_.min_longitudinal_offset_m);
 
   if (stop_obstacles.empty()) {
-    velocity_factors_pub_->publish(
-      obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time));
     // delete marker
     const auto markers =
       autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
@@ -336,8 +334,10 @@
 
     // Publish Stop Reason
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
-    velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-      planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose));
+    planning_factor_interface_->add(
+      output_traj_points, planner_data.ego_pose, stop_pose,
+      tier4_planning_msgs::msg::PlanningFactor::STOP,
+      tier4_planning_msgs::msg::SafetyFactorArray{});
     // Store stop reason debug data
     debug_data_ptr_->stop_metrics =
       makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle);
@@ -589,11 +589,15 @@
 
       const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
         slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down",
         planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward);
-      velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-        planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE,
-        slow_down_traj_points.at(slow_down_wall_idx).pose));
       autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker);
+      planning_factor_interface_->add(
+        slow_down_traj_points, planner_data.ego_pose,
+        slow_down_traj_points.at(*slow_down_start_idx).pose,
+        slow_down_traj_points.at(*slow_down_end_idx).pose,
+        tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN,
+        tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward,
+        stable_slow_down_vel);
     }
 
     // add debug virtual wall

planning/autoware_obstacle_cruise_planner/src/utils.cpp

@@ -118,25 +118,4 @@ std::vector<StopObstacle> getClosestStopObstacles(const std::vector<StopObstacle
   return candidates;
 }
 
-VelocityFactorArray makeVelocityFactorArray(
-  const rclcpp::Time & time, const std::string & behavior,
-  const std::optional<geometry_msgs::msg::Pose> pose)
-{
-  VelocityFactorArray velocity_factor_array;
-  velocity_factor_array.header.frame_id = "map";
-  velocity_factor_array.header.stamp = time;
-
-  if (pose) {
-    using distance_type = VelocityFactor::_distance_type;
-    VelocityFactor velocity_factor;
-    velocity_factor.behavior = behavior;
-    velocity_factor.pose = pose.value();
-    velocity_factor.distance = std::numeric_limits<distance_type>::quiet_NaN();
-    velocity_factor.status = VelocityFactor::UNKNOWN;
-    velocity_factor.detail = std::string();
-    velocity_factor_array.factors.push_back(velocity_factor);
-  }
-  return velocity_factor_array;
-}
-
 }  // namespace obstacle_cruise_utils

planning/autoware_surround_obstacle_checker/package.xml

@@ -14,7 +14,6 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
-  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>

planning/autoware_surround_obstacle_checker/src/debug_marker.cpp

@@ -67,7 +67,9 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
   const double & surround_check_back_distance, const double & surround_check_hysteresis_distance,
   const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock,
   rclcpp::Node & node)
-: vehicle_info_(vehicle_info),
+: planning_factor_interface_{std::make_unique<autoware::motion_utils::PlanningFactorInterface>(
+    &node, "surround_obstacle_checker")},
+  vehicle_info_(vehicle_info),
   object_label_(object_label),
   surround_check_front_distance_(surround_check_front_distance),
   surround_check_side_distance_(surround_check_side_distance),
@@ -77,8 +79,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
   clock_(clock)
 {
   debug_viz_pub_ = node.create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/marker", 1);
-  velocity_factor_pub_ =
-    node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/surround_obstacle", 1);
   vehicle_footprint_pub_ = node.create_publisher<PolygonStamped>("~/debug/footprint", 1);
   vehicle_footprint_offset_pub_ =
     node.create_publisher<PolygonStamped>("~/debug/footprint_offset", 1);
@@ -143,8 +143,12 @@ void SurroundObstacleCheckerDebugNode::publish()
   debug_viz_pub_->publish(visualization_msg);
 
   /* publish stop reason for autoware api */
-  const auto velocity_factor_msg = makeVelocityFactorArray();
-  velocity_factor_pub_->publish(velocity_factor_msg);
+  if (stop_pose_ptr_ != nullptr) {
+    planning_factor_interface_->add(
+      0.0, *stop_pose_ptr_, tier4_planning_msgs::msg::PlanningFactor::STOP,
+      tier4_planning_msgs::msg::SafetyFactorArray{});
+  }
+  planning_factor_interface_->publish();
 
   /* reset variables */
   stop_pose_ptr_ = nullptr;
@@ -170,25 +174,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker()
   return msg;
 }
 
-VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray()
-{
-  VelocityFactorArray velocity_factor_array;
-  velocity_factor_array.header.frame_id = "map";
-  velocity_factor_array.header.stamp = clock_->now();
-
-  if (stop_pose_ptr_) {
-    using distance_type = VelocityFactor::_distance_type;
-    VelocityFactor velocity_factor;
-    velocity_factor.behavior = PlanningBehavior::SURROUNDING_OBSTACLE;
-    velocity_factor.pose = *stop_pose_ptr_;
-    velocity_factor.distance = std::numeric_limits<distance_type>::quiet_NaN();
-    velocity_factor.status = VelocityFactor::UNKNOWN;
-    velocity_factor.detail = std::string();
-    velocity_factor_array.factors.push_back(velocity_factor);
-  }
-  return velocity_factor_array;
-}
-
 PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped(
   const Polygon2d & boost_polygon, const double & z)
 {