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 ea25be8e9d788..a4401be922426 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 @@ + @@ -174,6 +175,7 @@ + @@ -198,6 +200,7 @@ + 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..72e52ec71b9d7 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::shared_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_occulusion{false}; + double max_obj_vel_for_pseudo_occulusion{0.0}; + std::vector focus_intersections_for_pseudo_occulusion{}; }; 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..31f217293a693 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,11 +24,55 @@ #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 #include +#define debug(var) \ + do { \ + std::cerr << __LINE__ << ", " << __func__ << ", " << #var << ": "; \ + view(var); \ + } while (0) +template +void view(T e) +{ + std::cerr << e << std::endl; +} +template +void view(const std::vector & v) +{ + for (const auto & e : v) { + std::cerr << e << " "; + } + std::cerr << std::endl; +} +template +void view(const std::vector> & vv) +{ + for (const auto & v : vv) { + view(v); + } +} +#define line() \ + { \ + std::cerr << __LINE__ << ", " << __func__ << std::endl; \ + } +#define line_with_file() \ + { \ + std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \ + } + namespace { VelocityLimitClearCommand createVelocityLimitClearCommandMessage( @@ -275,6 +320,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_occulusion = node.declare_parameter( + "behavior_determination.slow_down.pseudo_occulusion.enable_function"); + if (work_as_pseudo_occulusion) { + max_obj_vel_for_pseudo_occulusion = node.declare_parameter( + "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel"); + focus_intersections_for_pseudo_occulusion = node.declare_parameter>( + "behavior_determination.slow_down.pseudo_occulusion.focus_intersections"); + } } void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( @@ -335,6 +388,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_occulusion.enable_function", + work_as_pseudo_occulusion); + if (work_as_pseudo_occulusion) { + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel", + max_obj_vel_for_pseudo_occulusion); + tier4_autoware_utils::updateParam>( + parameters, "behavior_determination.slow_down.pseudo_occulusion.focus_intersections", + focus_intersections_for_pseudo_occulusion); + } } ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) @@ -357,6 +421,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); @@ -486,9 +553,11 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms 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; } + route_handler_ = std::make_shared(*vector_map_ptr_); stop_watch_.tic(__func__); *debug_data_ptr_ = DebugData(); @@ -1130,6 +1199,42 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl return std::nullopt; } + const auto is_occulusion_object = [&]() { + if ( + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) > + behavior_determination_param_.max_obj_vel_for_pseudo_occulusion + 1e-6) { + line(); + return false; + } + + if (motion_utils::calcLateralOffset(traj_points, obstacle.pose.position) > 0.0) { + line(); + return true; + } + + for (const auto & id : + behavior_determination_param_.focus_intersections_for_pseudo_occulusion) { + 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)) { + line(); + return true; + } + } + line(); + return false; + }; + + if (behavior_determination_param_.work_as_pseudo_occulusion && !is_occulusion_object()) { + return std::nullopt; + } + // calculate front collision point double front_min_dist = std::numeric_limits::max(); geometry_msgs::msg::Point front_collision_point;