Merge pull request #406 from tier4/sync-upstream

chore: sync upstream

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -1784,6 +1784,30 @@ double calcYawDeviation(
 
   return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw);
 }
+
+/**
+ * @brief Check if the given target point is in front of the based pose from the trajectory.
+ * if the points is empty, the function returns false
+ * @param points Points of the trajectory, path, ...
+ * @param base_point Base point
+ * @param target_point Target point
+ * @param threshold threshold for judging front point
+ * @return true if the target pose is in front of the base pose
+ */
+template <class T>
+bool isTargetPointFront(
+  const T & points, const geometry_msgs::msg::Point & base_point,
+  const geometry_msgs::msg::Point & target_point, const double threshold = 0.0)
+{
+  if (points.empty()) {
+    return false;
+  }
+
+  const double s_base = calcSignedArcLength(points, 0, base_point);
+  const double s_target = calcSignedArcLength(points, 0, target_point);
+
+  return s_target - s_base > threshold;
+}
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -5452,3 +5452,101 @@ TEST(trajectory, calcYawDeviation)
     EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance);
   }
 }
+
+TEST(trajectory, isTargetPointFront)
+{
+  using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+  using motion_utils::isTargetPointFront;
+  using tier4_autoware_utils::createPoint;
+
+  // Generate test trajectory
+  const auto trajectory = generateTestTrajectory<Trajectory>(10, 1.0);
+
+  // Front point is base
+  {
+    const auto base_point = createPoint(5.0, 0.0, 0.0);
+    const auto target_point = createPoint(1.0, 0.0, 0.0);
+
+    EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point));
+  }
+
+  // Front point is target
+  {
+    const auto base_point = createPoint(1.0, 0.0, 0.0);
+    const auto target_point = createPoint(3.0, 0.0, 0.0);
+
+    EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point));
+  }
+
+  // boundary condition
+  {
+    const auto base_point = createPoint(1.0, 0.0, 0.0);
+    const auto target_point = createPoint(1.0, 0.0, 0.0);
+
+    EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point));
+  }
+
+  // before the start point
+  {
+    const auto base_point = createPoint(1.0, 0.0, 0.0);
+    const auto target_point = createPoint(-3.0, 0.0, 0.0);
+
+    EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point));
+  }
+
+  {
+    const auto base_point = createPoint(-5.0, 0.0, 0.0);
+    const auto target_point = createPoint(1.0, 0.0, 0.0);
+
+    EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point));
+  }
+
+  // after the end point
+  {
+    const auto base_point = createPoint(10.0, 0.0, 0.0);
+    const auto target_point = createPoint(3.0, 0.0, 0.0);
+
+    EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point));
+  }
+
+  {
+    const auto base_point = createPoint(2.0, 0.0, 0.0);
+    const auto target_point = createPoint(14.0, 0.0, 0.0);
+
+    EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point));
+  }
+
+  // empty point
+  {
+    const Trajectory traj;
+    const auto base_point = createPoint(2.0, 0.0, 0.0);
+    const auto target_point = createPoint(5.0, 0.0, 0.0);
+    EXPECT_FALSE(isTargetPointFront(traj.points, base_point, target_point));
+  }
+
+  // non-default threshold
+  {
+    const double threshold = 3.0;
+
+    {
+      const auto base_point = createPoint(5.0, 0.0, 0.0);
+      const auto target_point = createPoint(3.0, 0.0, 0.0);
+
+      EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold));
+    }
+
+    {
+      const auto base_point = createPoint(1.0, 0.0, 0.0);
+      const auto target_point = createPoint(4.0, 0.0, 0.0);
+
+      EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold));
+    }
+
+    {
+      const auto base_point = createPoint(1.0, 0.0, 0.0);
+      const auto target_point = createPoint(4.1, 0.0, 0.0);
+
+      EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point, threshold));
+    }
+  }
+}

planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp

@@ -19,6 +19,9 @@
 #include "behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <behaviortree_cpp_v3/behavior_tree.h>
@@ -81,6 +84,8 @@ class BehaviorTreeManager
     uint16_t max_msg_per_second = 25);
 
   void resetGrootMonitor();
+
+  bool isEgoOutOfRoute(const std::shared_ptr<PlannerData> & data) const;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -154,6 +154,7 @@ struct PlannerData
   OperationModeState::ConstSharedPtr operation_mode{};
   PathWithLaneId::SharedPtr reference_path{std::make_shared<PathWithLaneId>()};
   PathWithLaneId::SharedPtr prev_output_path{std::make_shared<PathWithLaneId>()};
+  std::optional<PoseWithUuidStamped> prev_modified_goal{};
   lanelet::ConstLanelets current_lanes{};
   std::shared_ptr<RouteHandler> route_handler{std::make_shared<RouteHandler>()};
   BehaviorPathPlannerParameters parameters{};

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -19,6 +19,7 @@
 #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
 #include "behavior_path_planner/utils/lane_following/module_data.hpp"
 
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -212,11 +213,17 @@ class PlannerManager
       root_lanelet_.get(), pose, backward_length, std::numeric_limits<double>::max());
 
     lanelet::ConstLanelet closest_lane{};
-    if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) {
-      return {};
+    if (lanelet::utils::query::getClosestLaneletWithConstrains(
+          lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold,
+          p.ego_nearest_yaw_threshold)) {
+      return utils::getReferencePath(closest_lane, data);
     }
 
-    return utils::getReferencePath(closest_lane, data);
+    if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) {
+      return utils::getReferencePath(closest_lane, data);
+    }
+
+    return {};  // something wrong.
   }
 
   /**
@@ -347,6 +354,8 @@ class PlannerManager
     const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
     const BehaviorModuleOutput & previous_module_output);
 
+  bool isEgoOutOfRoute(const std::shared_ptr<PlannerData> & data) const;
+
   boost::optional<lanelet::ConstLanelet> root_lanelet_{boost::none};
 
   std::vector<SceneModuleManagerPtr> manager_ptrs_;

planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp

@@ -55,6 +55,10 @@ struct ProjectedDistancePoint
   double distance{0.0};
 };
 
+bool isTargetObjectFront(
+  const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose,
+  const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon);
+
 /**
  * @brief Project nearest point on a line segment.
  * @param [in] reference_point point to project

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -292,6 +292,12 @@ PathWithLaneId refinePathForGoal(
 
 bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id);
 
+PathWithLaneId createGoalAroundPath(
+  const std::shared_ptr<RouteHandler> & route_handler,
+  const std::optional<PoseWithUuidStamped> & modified_goal);
+
+bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes);
+
 // path management
 
 // TODO(Horibe) There is a similar function in route_handler. Check.

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1152,6 +1152,7 @@ void BehaviorPathPlannerNode::run()
   if (output.modified_goal) {
     PoseWithUuidStamped modified_goal = *(output.modified_goal);
     modified_goal.header.stamp = path->header.stamp;
+    planner_data_->prev_modified_goal = modified_goal;
     modified_goal_publisher_->publish(modified_goal);
   }
 

planning/behavior_path_planner/src/behavior_tree_manager.cpp

@@ -89,6 +89,15 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr<PlannerData>
     *s = SceneModuleStatus{s->module_name};
   });
 
+  if (isEgoOutOfRoute(data)) {
+    BehaviorModuleOutput output{};
+    const auto output_path =
+      utils::createGoalAroundPath(data->route_handler, data->prev_modified_goal);
+    output.path = std::make_shared<PathWithLaneId>(output_path);
+    output.reference_path = std::make_shared<PathWithLaneId>(output_path);
+    return output;
+  }
+
   // reset blackboard
   blackboard_->set<BehaviorModuleOutput>("output", BehaviorModuleOutput{});
 
@@ -150,6 +159,51 @@ void BehaviorTreeManager::resetGrootMonitor()
   }
 }
 
+bool BehaviorTreeManager::isEgoOutOfRoute(const std::shared_ptr<PlannerData> & data) const
+{
+  const auto self_pose = data->self_odometry->pose.pose;
+  const Pose goal_pose =
+    data->prev_modified_goal ? data->prev_modified_goal->pose : data->route_handler->getGoalPose();
+  const auto shoulder_lanes = data->route_handler->getShoulderLanelets();
+
+  lanelet::ConstLanelet goal_lane;
+  const bool is_failed_getting_lanelet = std::invoke([&]() {
+    if (utils::isInLanelets(goal_pose, shoulder_lanes)) {
+      return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane);
+    }
+    return !data->route_handler->getGoalLanelet(&goal_lane);
+  });
+  if (is_failed_getting_lanelet) {
+    RCLCPP_WARN_STREAM(logger_, "cannot find goal lanelet");
+    return true;
+  }
+
+  // If ego vehicle is over goal on goal lane, return true
+  if (lanelet::utils::isInLanelet(self_pose, goal_lane)) {
+    constexpr double buffer = 1.0;
+    const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose);
+    const auto goal_arc_coord =
+      lanelet::utils::getArcCoordinates({goal_lane}, data->route_handler->getGoalPose());
+    if (ego_arc_coord.length > goal_arc_coord.length + buffer) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  // If ego vehicle is out of the closest lanelet, return true
+  lanelet::ConstLanelet closest_lane;
+  if (!data->route_handler->getClosestLaneletWithinRoute(self_pose, &closest_lane)) {
+    RCLCPP_WARN_STREAM(logger_, "cannot find closest lanelet");
+    return true;
+  }
+  if (!lanelet::utils::isInLanelet(self_pose, closest_lane)) {
+    return true;
+  }
+
+  return false;
+}
+
 std::shared_ptr<SceneModuleVisitor> BehaviorTreeManager::getAllSceneModuleDebugMsgData()
 {
   scene_module_visitor_ptr_ = std::make_shared<SceneModuleVisitor>();

planning/behavior_path_planner/src/planner_manager.cpp

@@ -26,7 +26,6 @@
 
 namespace behavior_path_planner
 {
-
 PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose)
 : logger_(node.get_logger().get_child("planner_manager")),
   clock_(*node.get_clock()),
@@ -48,6 +47,14 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
     manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); });
 
   auto result_output = [&]() {
+    if (isEgoOutOfRoute(data)) {
+      BehaviorModuleOutput output{};
+      const auto output_path =
+        utils::createGoalAroundPath(data->route_handler, data->prev_modified_goal);
+      output.path = std::make_shared<PathWithLaneId>(output_path);
+      output.reference_path = std::make_shared<PathWithLaneId>(output_path);
+      return output;
+    }
     while (rclcpp::ok()) {
       /**
        * STEP1: get approved modules' output
@@ -606,6 +613,51 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr<PlannerData> & data)
   RCLCPP_INFO_EXPRESSION(logger_, verbose_, "change ego's following lane. reset.");
 }
 
+bool PlannerManager::isEgoOutOfRoute(const std::shared_ptr<PlannerData> & data) const
+{
+  const auto self_pose = data->self_odometry->pose.pose;
+  const Pose goal_pose =
+    data->prev_modified_goal ? data->prev_modified_goal->pose : data->route_handler->getGoalPose();
+  const auto shoulder_lanes = data->route_handler->getShoulderLanelets();
+
+  lanelet::ConstLanelet goal_lane;
+  const bool is_failed_getting_lanelet = std::invoke([&]() {
+    if (utils::isInLanelets(goal_pose, shoulder_lanes)) {
+      return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane);
+    }
+    return !data->route_handler->getGoalLanelet(&goal_lane);
+  });
+  if (is_failed_getting_lanelet) {
+    RCLCPP_WARN_STREAM(logger_, "cannot find goal lanelet");
+    return true;
+  }
+
+  // If ego vehicle is over goal on goal lane, return true
+  if (lanelet::utils::isInLanelet(self_pose, goal_lane)) {
+    constexpr double buffer = 1.0;
+    const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose);
+    const auto goal_arc_coord =
+      lanelet::utils::getArcCoordinates({goal_lane}, data->route_handler->getGoalPose());
+    if (ego_arc_coord.length > goal_arc_coord.length + buffer) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  // If ego vehicle is out of the closest lanelet, return true
+  lanelet::ConstLanelet closest_lane;
+  if (!data->route_handler->getClosestLaneletWithinRoute(self_pose, &closest_lane)) {
+    RCLCPP_WARN_STREAM(logger_, "cannot find closest lanelet");
+    return true;
+  }
+  if (!lanelet::utils::isInLanelet(self_pose, closest_lane)) {
+    return true;
+  }
+
+  return false;
+}
+
 void PlannerManager::print() const
 {
   if (!verbose_) {