diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
index 18de04fd9e317..ece5440bd06bd 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml
@@ -125,6 +125,7 @@
+
@@ -165,6 +166,56 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
index 4a3654d48afe1..ed25ee6f4e996 100644
--- 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::SharedPtr objects_sub_;
rclcpp::Subscription::SharedPtr odom_sub_;
rclcpp::Subscription::SharedPtr acc_sub_;
+ rclcpp::Subscription::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_;
// 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 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
index d46062dd8f85c..d555650b07690 100644
--- 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
index a6c95d65acc6a..5775f1b9a2f1d 100644
--- 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 @@
+
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index 31f011b3749b9..37c567a9637b8 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -16,6 +16,7 @@
#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"
@@ -23,7 +24,16 @@
#include "tier4_autoware_utils/ros/marker_helper.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"
+#include
+#include
+#include
+
#include
+#include
+#include
+
+#include
+#include
#include
#include
@@ -275,6 +285,14 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
"behavior_determination.consider_current_pose.enable_to_consider_current_pose");
time_to_convergence = node.declare_parameter(
"behavior_determination.consider_current_pose.time_to_convergence");
+ work_as_pseudo_occlusion = node.declare_parameter(
+ "behavior_determination.slow_down.pseudo_occlusion.enable_function");
+ if (work_as_pseudo_occlusion) {
+ max_obj_vel_for_pseudo_occlusion = node.declare_parameter(
+ "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel");
+ focus_intersections_for_pseudo_occlusion = node.declare_parameter>(
+ "behavior_determination.slow_down.pseudo_occlusion.focus_intersections");
+ }
}
void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
@@ -335,6 +353,17 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
tier4_autoware_utils::updateParam(
parameters, "behavior_determination.consider_current_pose.time_to_convergence",
time_to_convergence);
+ tier4_autoware_utils::updateParam(
+ parameters, "behavior_determination.slow_down.pseudo_occlusion.enable_function",
+ work_as_pseudo_occlusion);
+ if (work_as_pseudo_occlusion) {
+ tier4_autoware_utils::updateParam(
+ parameters, "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel",
+ max_obj_vel_for_pseudo_occlusion);
+ tier4_autoware_utils::updateParam>(
+ 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(
"~/input/acceleration", rclcpp::QoS{1},
[this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { ego_accel_ptr_ = msg; });
+ lanelet_map_sub_ = create_subscription(
+ "~/input/vector_map", rclcpp::QoS(10).transient_local(),
+ [this](const HADMapBin::ConstSharedPtr msg) { vector_map_ptr_ = msg; });
// publisher
trajectory_pub_ = create_publisher("~/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(*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 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::max();
geometry_msgs::msg::Point front_collision_point;