feat(obstacle_cruise): pseudo occlusion (#1258)

Signed-off-by: Yuki Takagi <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Makoto Kurihara <[email protected]>
tier4 · May 10, 2024 · cb38bda · cb38bda
1 parent 8c29188
commit cb38bda
Show file tree

Hide file tree

Showing 5 changed files with 131 additions and 2 deletions.
diff --git a/...g_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/...g_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -125,6 +125,7 @@
           <remap from="~/input/odometry" to="/localization/kinematic_state"/>
           <remap from="~/input/acceleration" to="/localization/acceleration"/>
           <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+          <remap from="~/input/vector_map" to="/map/vector_map"/>
           <remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
           <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
           <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
@@ -165,6 +166,56 @@
       </load_composable_node>
     </group>
 
+    <!-- First Obstacle Cruise (original) -->
+    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'&quot;)">
+      <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
+        <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner" namespace="">
+          <!-- topic remap -->
+          <remap from="~/input/trajectory" to="obstacle_velocity_limiter/trajectory"/>
+          <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+          <remap from="~/input/acceleration" to="/localization/acceleration"/>
+          <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+          <remap from="~/input/vector_map" to="/map/vector_map"/>
+          <remap from="~/output/trajectory" to="obstacle_cruise/trajectory"/>
+          <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
+          <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
+          <remap from="~/output/clear_velocity_limit" to="/planning/scenario_planning/clear_velocity_limit"/>
+          <!-- params -->
+          <param from="$(var common_param_path)"/>
+          <param from="$(var vehicle_param_file)"/>
+          <param from="$(var nearest_search_param_path)"/>
+          <param from="$(var obstacle_cruise_planner_param_path)"/>
+          <!-- composable node config -->
+          <extra_arg name="use_intra_process_comms" value="false"/>
+        </composable_node>
+      </load_composable_node>
+    </group>
+
+    <!-- Second Obstacle Cruise (pseudo occlusion)-->
+    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'&quot;)">
+      <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
+        <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner_pseudo_occlusion" namespace="">
+          <!-- topic remap -->
+          <remap from="~/input/trajectory" to="obstacle_cruise/trajectory"/>
+          <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+          <remap from="~/input/acceleration" to="/localization/acceleration"/>
+          <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
+          <remap from="~/input/vector_map" to="/map/vector_map"/>
+          <remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
+          <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
+          <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
+          <remap from="~/output/clear_velocity_limit" to="/planning/scenario_planning/clear_velocity_limit"/>
+          <!-- params -->
+          <param from="$(var common_param_path)"/>
+          <param from="$(var vehicle_param_file)"/>
+          <param from="$(var nearest_search_param_path)"/>
+          <param from="$(var obstacle_cruise_planner_pseudo_occlusion_param_path)"/>
+          <!-- composable node config -->
+          <extra_arg name="use_intra_process_comms" value="false"/>
+        </composable_node>
+      </load_composable_node>
+    </group>
+
     <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'none'&quot;)">
       <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
         <composable_node pkg="topic_tools" plugin="topic_tools::RelayNode" name="obstacle_stop_relay" namespace="">

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -19,6 +19,7 @@
 #include "obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp"
 #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp"
 #include "obstacle_cruise_planner/type_alias.hpp"
+#include "route_handler/route_handler.hpp"
 #include "signal_processing/lowpass_filter_1d.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
@@ -136,11 +137,15 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
   rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
   rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;
+  rclcpp::Subscription<HADMapBin>::SharedPtr lanelet_map_sub_;
 
   // data for callback functions
   PredictedObjects::ConstSharedPtr objects_ptr_{nullptr};
   Odometry::ConstSharedPtr ego_odom_ptr_{nullptr};
   AccelWithCovarianceStamped::ConstSharedPtr ego_accel_ptr_{nullptr};
+  HADMapBin::ConstSharedPtr vector_map_ptr_{nullptr};
+
+  std::unique_ptr<route_handler::RouteHandler> route_handler_;
 
   // Vehicle Parameters
   VehicleInfo vehicle_info_;
@@ -198,6 +203,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     // consideration for the current ego pose
     bool enable_to_consider_current_pose{false};
     double time_to_convergence{1.5};
+    bool work_as_pseudo_occlusion{false};
+    double max_obj_vel_for_pseudo_occlusion{0.0};
+    std::vector<lanelet::Id> focus_intersections_for_pseudo_occlusion{};
   };
   BehaviorDeterminationParam behavior_determination_param_;
 

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp
@@ -19,6 +19,7 @@
 
 #include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp"
 #include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp"
+#include "autoware_auto_mapping_msgs/msg/had_map_bin.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/trajectory.hpp"
@@ -38,6 +39,7 @@
 using autoware_adapi_v1_msgs::msg::PlanningBehavior;
 using autoware_adapi_v1_msgs::msg::VelocityFactor;
 using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
+using autoware_auto_mapping_msgs::msg::HADMapBin;
 using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;

diff --git a/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml
@@ -26,6 +26,7 @@
     <remap from="~/input/trajectory" to="$(var input_trajectory)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
     <remap from="~/input/objects" to="$(var input_objects)"/>
+    <remap from="~/input/vector_map" to="$(var input_map)"/>
 
     <!-- output -->
     <remap from="~/output/trajectory" to="$(var output_trajectory)"/>

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -16,14 +16,24 @@
 
 #include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/tmp_conversion.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
 #include "object_recognition_utils/predicted_path_utils.hpp"
 #include "obstacle_cruise_planner/polygon_utils.hpp"
 #include "obstacle_cruise_planner/utils.hpp"
 #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
 #include "tier4_autoware_utils/ros/marker_helper.hpp"
 #include "tier4_autoware_utils/ros/update_param.hpp"
 
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+
 #include <boost/format.hpp>
+#include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/algorithms/within.hpp>
+
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_core/geometry/Polygon.h>
 
 #include <algorithm>
 #include <chrono>
@@ -275,6 +285,14 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
     "behavior_determination.consider_current_pose.enable_to_consider_current_pose");
   time_to_convergence = node.declare_parameter<double>(
     "behavior_determination.consider_current_pose.time_to_convergence");
+  work_as_pseudo_occlusion = node.declare_parameter<bool>(
+    "behavior_determination.slow_down.pseudo_occlusion.enable_function");
+  if (work_as_pseudo_occlusion) {
+    max_obj_vel_for_pseudo_occlusion = node.declare_parameter<double>(
+      "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel");
+    focus_intersections_for_pseudo_occlusion = node.declare_parameter<std::vector<lanelet::Id>>(
+      "behavior_determination.slow_down.pseudo_occlusion.focus_intersections");
+  }
 }
 
 void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
@@ -335,6 +353,17 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
   tier4_autoware_utils::updateParam<double>(
     parameters, "behavior_determination.consider_current_pose.time_to_convergence",
     time_to_convergence);
+  tier4_autoware_utils::updateParam<bool>(
+    parameters, "behavior_determination.slow_down.pseudo_occlusion.enable_function",
+    work_as_pseudo_occlusion);
+  if (work_as_pseudo_occlusion) {
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel",
+      max_obj_vel_for_pseudo_occlusion);
+    tier4_autoware_utils::updateParam<std::vector<lanelet::Id>>(
+      parameters, "behavior_determination.slow_down.pseudo_occlusion.focus_intersections",
+      focus_intersections_for_pseudo_occlusion);
+  }
 }
 
 ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
@@ -357,6 +386,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
   acc_sub_ = create_subscription<AccelWithCovarianceStamped>(
     "~/input/acceleration", rclcpp::QoS{1},
     [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { ego_accel_ptr_ = msg; });
+  lanelet_map_sub_ = create_subscription<HADMapBin>(
+    "~/input/vector_map", rclcpp::QoS(10).transient_local(),
+    [this](const HADMapBin::ConstSharedPtr msg) { vector_map_ptr_ = msg; });
 
   // publisher
   trajectory_pub_ = create_publisher<Trajectory>("~/output/trajectory", 1);
@@ -483,14 +515,18 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
 
 void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg)
 {
+  stop_watch_.tic(__func__);
   const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg);
 
   // check if subscribed variables are ready
-  if (traj_points.empty() || !ego_odom_ptr_ || !ego_accel_ptr_ || !objects_ptr_) {
+  if (
+    traj_points.empty() || !ego_odom_ptr_ || !ego_accel_ptr_ || !objects_ptr_ || !vector_map_ptr_) {
     return;
   }
+  if (route_handler_ == nullptr) {
+    route_handler_ = std::make_unique<route_handler::RouteHandler>(*vector_map_ptr_);
+  }
 
-  stop_watch_.tic(__func__);
   *debug_data_ptr_ = DebugData();
 
   const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(traj_points);
@@ -1130,6 +1166,37 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
     return std::nullopt;
   }
 
+  const auto is_occlusion_object = [&]() {
+    if (
+      std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) >
+      behavior_determination_param_.max_obj_vel_for_pseudo_occlusion + 1e-6) {
+      return false;
+    }
+
+    if (motion_utils::calcLateralOffset(traj_points, obstacle.pose.position) > 0.0) {
+      return true;
+    }
+
+    for (const auto & id : behavior_determination_param_.focus_intersections_for_pseudo_occlusion) {
+      if (id == 0) {
+        continue;
+      }
+      const auto intersection_poly =
+        lanelet::utils::to2D(route_handler_->getLaneletMapPtr()->polygonLayer.get(id))
+          .basicPolygon();
+      if (
+        boost::geometry::within(obstacle_poly, intersection_poly) ||
+        boost::geometry::intersects(obstacle_poly, intersection_poly)) {
+        return true;
+      }
+    }
+    return false;
+  };
+
+  if (behavior_determination_param_.work_as_pseudo_occlusion && !is_occlusion_object()) {
+    return std::nullopt;
+  }
+
   // calculate front collision point
   double front_min_dist = std::numeric_limits<double>::max();
   geometry_msgs::msg::Point front_collision_point;