add visitor

soblin · soblin · commit 4a157497dc0e · 2024-05-24T13:31:09.000+09:00
Signed-off-by: Mamoru Sobue &lt;mamoru.sobue@tier4.jp&gt;
diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt
@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/debug.cpp
   src/manager.cpp
   src/scene.cpp
+  src/decisions.cpp
 )
 
 ament_auto_package(INSTALL_TO_SHARE config)
diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp
@@ -0,0 +1,154 @@
+// Copyright 2024 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "scene.hpp"
+
+#include <motion_utils/trajectory/trajectory.hpp>
+
+namespace behavior_velocity_planner
+{
+
+/*
+ * for default
+ */
+template <typename T>
+void BlindSpotModule::setRTCStatusByDecision(
+  const T &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  static_assert("Unsupported type passed to setRTCStatus");
+  return;
+}
+
+template <typename T>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  [[maybe_unused]] const T & decision,
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  [[maybe_unused]] StopReason * stop_reason)
+{
+  static_assert("Unsupported type passed to reactRTCApprovalByDecision");
+}
+
+/*
+ * for InternalError
+ */
+template <>
+void BlindSpotModule::setRTCStatusByDecision(
+  [[maybe_unused]] const InternalError & decision,
+  [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  return;
+}
+
+template <>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  [[maybe_unused]] const InternalError & decision,
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  [[maybe_unused]] StopReason * stop_reason)
+{
+  return;
+}
+
+/*
+ * For OverPassJudge
+ */
+template <>
+void BlindSpotModule::setRTCStatusByDecision(
+  [[maybe_unused]] const OverPassJudge & decision,
+  [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  return;
+}
+
+template <>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  [[maybe_unused]] const OverPassJudge & decision,
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  [[maybe_unused]] StopReason * stop_reason)
+{
+  return;
+}
+
+/*
+ * for Unsafe
+ */
+template <>
+void BlindSpotModule::setRTCStatusByDecision(
+  const Unsafe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  setSafe(false);
+  const auto & current_pose = planner_data_->current_odometry->pose;
+  setDistance(
+    motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx));
+  return;
+}
+
+template <>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  const Unsafe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  StopReason * stop_reason)
+{
+  if (!isActivated()) {
+    constexpr double stop_vel = 0.0;
+    planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
+    debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
+      decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
+
+    tier4_planning_msgs::msg::StopFactor stop_factor;
+    const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
+    stop_factor.stop_pose = stop_pose;
+    stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
+    planning_utils::appendStopReason(stop_factor, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
+  }
+  return;
+}
+
+/*
+ * for Safe
+ */
+template <>
+void BlindSpotModule::setRTCStatusByDecision(
+  const Safe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  setSafe(true);
+  const auto & current_pose = planner_data_->current_odometry->pose;
+  setDistance(
+    motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx));
+  return;
+}
+
+template <>
+void BlindSpotModule::reactRTCApprovalByDecision(
+  const Safe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+  StopReason * stop_reason)
+{
+  if (!isActivated()) {
+    constexpr double stop_vel = 0.0;
+    planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
+    debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
+      decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
+
+    tier4_planning_msgs::msg::StopFactor stop_factor;
+    const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
+    stop_factor.stop_pose = stop_pose;
+    stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
+    planning_utils::appendStopReason(stop_factor, stop_reason);
+    velocity_factor_.set(
+      path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
+  }
+  return;
+}
+
+}  // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp
@@ -72,8 +72,8 @@ void BlindSpotModuleManager::launchNewModules(
                                   : BlindSpotModule::TurnDirection::RIGHT;
 
     registerModule(std::make_shared<BlindSpotModule>(
-      module_id, lane_id, turn_direction, planner_param_, logger_.get_child("blind_spot_module"),
-      clock_));
+      module_id, lane_id, turn_direction, planner_data_, planner_param_,
+      logger_.get_child("blind_spot_module"), clock_));
     generateUUID(module_id);
     updateRTCStatus(
       getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits<double>::lowest(),
diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
@@ -44,36 +44,18 @@ namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
 
-namespace
-{
-[[maybe_unused]] geometry_msgs::msg::Polygon toGeomPoly(const lanelet::CompoundPolygon3d & poly)
-{
-  geometry_msgs::msg::Polygon geom_poly;
-
-  for (const auto & p : poly) {
-    geometry_msgs::msg::Point32 geom_point;
-    geom_point.x = p.x();
-    geom_point.y = p.y();
-    geom_point.z = p.z();
-    geom_poly.points.push_back(geom_point);
-  }
-
-  return geom_poly;
-}
-}  // namespace
-
 BlindSpotModule::BlindSpotModule(
   const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction,
-  const PlannerParam & planner_param, const rclcpp::Logger logger,
-  const rclcpp::Clock::SharedPtr clock)
+  const std::shared_ptr<const PlannerData> planner_data, const PlannerParam & planner_param,
+  const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
   planner_param_{planner_param},
   turn_direction_(turn_direction),
   is_over_pass_judge_line_(false)
 {
   velocity_factor_.init(PlanningBehavior::REAR_CHECK);
-  sibling_straight_lanelet_ = getSiblingStraightLanelet();
+  sibling_straight_lanelet_ = getSiblingStraightLanelet(planner_data);
 }
 
 void BlindSpotModule::initializeRTCStatus()
@@ -130,33 +112,39 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
     collision_obstacle.has_value() ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("state_machine"), *clock_);
 
-  if (state_machine_.getState() != StateMachine::State::STOP) {
+  if (state_machine_.getState() == StateMachine::State::STOP) {
     return Unsafe{stop_line_idx, collision_obstacle};
   }
 
-  return Safe{};
-  /* set stop speed */
-  /*
-  setSafe(state_machine_.getState() != StateMachine::State::STOP);
-  setDistance(motion_utils::calcSignedArcLength(
-    input_path.points, current_pose.position,
-    input_path.points.at(stop_line_idx).point.pose.position));
-  */
-  /*
-  if (!isActivated()) {
-    constexpr double stop_vel = 0.0;
-    planning_utils::setVelocityFromIndex(stop_line_idx, stop_vel, path);
-    debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
-    stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, input_path);
+  return Safe{stop_line_idx};
+}
 
-    tier4_planning_msgs::msg::StopFactor stop_factor;
-    stop_factor.stop_pose = debug_data_.stop_point_pose;  // stop_point_poseを消したので直す
-    stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
-    planning_utils::appendStopReason(stop_factor, stop_reason);
-    velocity_factor_.set(
-      input_path.points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
-  }
-  */
+// template-specification based visitor pattern
+// https://en.cppreference.com/w/cpp/utility/variant/visit
+template <class... Ts>
+struct VisitorSwitch : Ts...
+{
+  using Ts::operator()...;
+};
+template <class... Ts>
+VisitorSwitch(Ts...) -> VisitorSwitch<Ts...>;
+
+void BlindSpotModule::setRTCStatus(
+  const BlindSpotDecision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
+{
+  std::visit(
+    VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }},
+    decision);
+}
+
+void BlindSpotModule::reactRTCApproval(
+  const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason)
+{
+  std::visit(
+    VisitorSwitch{[&](const auto & sub_decision) {
+      reactRTCApprovalByDecision(sub_decision, path, stop_reason);
+    }},
+    decision);
 }
 
 bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
@@ -166,6 +154,9 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
 
   initializeRTCStatus();
   const auto decision = modifyPathVelocityDetail(path, stop_reason);
+  const auto & input_path = *path;
+  setRTCStatus(decision, input_path);
+  reactRTCApproval(decision, path, stop_reason);
 
   return true;
 }
@@ -223,10 +214,11 @@ std::optional<InterpolatedPathInfo> BlindSpotModule::generateInterpolatedPathInf
   return interpolated_path_info;
 }
 
-std::optional<lanelet::ConstLanelet> BlindSpotModule::getSiblingStraightLanelet() const
+std::optional<lanelet::ConstLanelet> BlindSpotModule::getSiblingStraightLanelet(
+  const std::shared_ptr<const PlannerData> planner_data) const
 {
-  const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
-  const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
+  const auto lanelet_map_ptr = planner_data->route_handler_->getLaneletMapPtr();
+  const auto routing_graph_ptr = planner_data->route_handler_->getRoutingGraphPtr();
 
   const auto assigned_lane = lanelet_map_ptr->laneletLayer.get(lane_id_);
   for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) {
diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp
@@ -78,9 +78,9 @@ struct Unsafe
   const std::optional<autoware_auto_perception_msgs::msg::PredictedObject> collision_obstacle;
 };
 
-struct Safe : public std::monostate
+struct Safe
 {
-  using std::monostate::monostate;
+  const size_t stop_line_idx;
 };
 
 using BlindSpotDecision = std::variant<InternalError, OverPassJudge, Unsafe, Safe>;
@@ -117,8 +117,8 @@ class BlindSpotModule : public SceneModuleInterface
 
   BlindSpotModule(
     const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction,
-    const PlannerParam & planner_param, const rclcpp::Logger logger,
-    const rclcpp::Clock::SharedPtr clock);
+    const std::shared_ptr<const PlannerData> planner_data, const PlannerParam & planner_param,
+    const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
 
   /**
    * @brief plan go-stop velocity at traffic crossing with collision check between reference path
@@ -144,18 +144,25 @@ class BlindSpotModule : public SceneModuleInterface
   void initializeRTCStatus();
   BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason);
   // setDafe(), setDistance()
-  void setRTCStatus(const BlindSpotDecision & decision);
+  void setRTCStatus(
+    const BlindSpotDecision & decision,
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
   template <typename Decision>
-  void setRTCStatusByDecision(const Decision & decision);
+  void setRTCStatusByDecision(
+    const Decision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
   // stop/GO
-  void reactRTCApproval(PathWithLaneId * path, StopReason * stop_reason) const;
+  void reactRTCApproval(
+    const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason);
   template <typename Decision>
-  void reactRTCApprovalByDecision(const Decision & decision) const;
+  void reactRTCApprovalByDecision(
+    const Decision & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
+    StopReason * stop_reason);
 
   std::optional<InterpolatedPathInfo> generateInterpolatedPathInfo(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const;
 
-  std::optional<lanelet::ConstLanelet> getSiblingStraightLanelet() const;
+  std::optional<lanelet::ConstLanelet> getSiblingStraightLanelet(
+    const std::shared_ptr<const PlannerData> planner_data) const;
 
   /**
    * @brief Generate a stop line and insert it into the path.

Original file line number	Diff line number	Diff line change
`@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED`
`9`	`9`	`src/debug.cpp`
`10`	`10`	`src/manager.cpp`
`11`	`11`	`src/scene.cpp`
	`12`	`+ src/decisions.cpp`
`12`	`13`	`)`
`13`	`14`
`14`	`15`	`ament_auto_package(INSTALL_TO_SHARE config)`