Merge branch 'beta/v0.29.0' into exp/beta/v0.29.0-semseg-filter

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp

@@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg:
   const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
   const double threshold = 0.0);
 
+/// @brief calculate the time_from_start fields of the given trajectory points
+/// @details this function assumes constant longitudinal velocity between points
+/// @param trajectory trajectory for which to calculate the time_from_start
+/// @param current_ego_point current ego position
+/// @param min_velocity minimum velocity used for a trajectory point
+void calculate_time_from_start(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f);
+
 }  // namespace autoware::motion_utils
 
 #endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

common/autoware_motion_utils/src/resample/resample.cpp

@@ -21,6 +21,8 @@
 #include "interpolation/spline_interpolation.hpp"
 #include "interpolation/zero_order_hold.hpp"
 
+#include <cstdlib>
+
 namespace autoware::motion_utils
 {
 std::vector<geometry_msgs::msg::Point> resamplePointVector(
@@ -601,11 +603,13 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory(
   rear_wheel_angle.push_back(input_trajectory.points.front().rear_wheel_angle_rad);
   time_from_start.push_back(
     rclcpp::Duration(input_trajectory.points.front().time_from_start).seconds());
+
   for (size_t i = 1; i < input_trajectory.points.size(); ++i) {
     const auto & prev_pt = input_trajectory.points.at(i - 1);
     const auto & curr_pt = input_trajectory.points.at(i);
     const double ds =
       autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position);
+
     input_arclength.push_back(ds + input_arclength.back());
     input_pose.push_back(curr_pt.pose);
     v_lon.push_back(curr_pt.longitudinal_velocity_mps);
@@ -617,6 +621,19 @@ autoware_planning_msgs::msg::Trajectory resampleTrajectory(
     time_from_start.push_back(rclcpp::Duration(curr_pt.time_from_start).seconds());
   }
 
+  // Set Zero Velocity After Stop Point
+  // If the longitudinal velocity is zero, set the velocity to zero after that point.
+  bool stop_point_found_in_v_lon = false;
+  constexpr double epsilon = 1e-4;
+  for (size_t i = 0; i < v_lon.size(); ++i) {
+    if (std::abs(v_lon.at(i)) < epsilon) {
+      stop_point_found_in_v_lon = true;
+    }
+    if (stop_point_found_in_v_lon) {
+      v_lon.at(i) = 0.0;
+    }
+  }
+
   // Interpolate
   const auto lerp = [&](const auto & input) {
     return interpolation::lerp(input_arclength, input, resampled_arclength);

common/autoware_motion_utils/src/trajectory/trajectory.cpp

@@ -599,4 +599,27 @@ template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg::Trajec
   const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
   const double threshold);
 
+void calculate_time_from_start(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const float min_velocity)
+{
+  const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point);
+  if (nearest_segment_idx + 1 == trajectory.size()) {
+    return;
+  }
+  for (auto & p : trajectory) {
+    p.time_from_start = rclcpp::Duration::from_seconds(0);
+  }
+  // TODO(Maxime): some points can have very low velocities which introduce huge time errors
+  // Temporary solution: use a minimum velocity
+  for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) {
+    const auto & from = trajectory[idx - 1];
+    const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps);
+    if (velocity != 0.0) {
+      auto & to = trajectory[idx];
+      const auto t = universe_utils::calcDistance2d(from, to) / velocity;
+      to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start;
+    }
+  }
+}
 }  // namespace autoware::motion_utils

planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -17,6 +17,7 @@
 #include "utility_functions.hpp"
 
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/normalization.hpp>
 #include <autoware/universe_utils/math/unit_conversion.hpp>
@@ -27,10 +28,13 @@
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <lanelet2_extension/visualization/visualization.hpp>
 
+#include <boost/geometry/algorithms/difference.hpp>
+#include <boost/geometry/algorithms/is_empty.hpp>
+
+#include <lanelet2_core/Forward.h>
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
 #include <lanelet2_core/geometry/Lanelet.h>
-#include <lanelet2_routing/Route.h>
-#include <lanelet2_routing/RoutingCost.h>
 #include <tf2/utils.h>
 
 #include <limits>
@@ -216,7 +220,7 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
 }
 
 visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
-  autoware::universe_utils::LinearRing2d goal_footprint) const
+  autoware::universe_utils::LinearRing2d goal_footprint)
 {
   visualization_msgs::msg::MarkerArray msg;
   auto marker = autoware::universe_utils::createDefaultMarker(
@@ -244,52 +248,58 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
   return msg;
 }
 
-bool DefaultPlanner::check_goal_footprint_inside_lanes(
-  const lanelet::ConstLanelet & current_lanelet,
-  const lanelet::ConstLanelet & combined_prev_lanelet,
-  const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length,
-  const double search_margin)
+lanelet::ConstLanelets next_lanelets_up_to(
+  const lanelet::ConstLanelet & start_lanelet, const double up_to_distance,
+  const route_handler::RouteHandler & route_handler)
 {
-  // check if goal footprint is in current lane
-  if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) {
-    return true;
+  lanelet::ConstLanelets lanelets;
+  if (up_to_distance <= 0.0) {
+    return lanelets;
   }
-  const auto following = route_handler_.getNextLanelets(current_lanelet);
-  // check if goal footprint is in between many lanelets in depth-first search manner
-  for (const auto & next_lane : following) {
-    next_lane_length += lanelet::utils::getLaneletLength2d(next_lane);
-    lanelet::ConstLanelets lanelets;
-    lanelets.push_back(combined_prev_lanelet);
+  for (const auto & next_lane : route_handler.getNextLanelets(start_lanelet)) {
     lanelets.push_back(next_lane);
-    lanelet::ConstLanelet combined_lanelets =
-      combine_lanelets_with_shoulder(lanelets, route_handler_);
-
-    // if next lanelet length is longer than vehicle longitudinal offset
-    if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
-      next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
-      // and if the goal_footprint is within the (accumulated) combined_lanelets, terminate the
-      // query
-      if (boost::geometry::within(goal_footprint, combined_lanelets.polygon2d().basicPolygon())) {
-        return true;
-      }
-      // if not, iteration continues to next next_lane, and this subtree is terminated
-    } else {  // if next lanelet length is shorter than vehicle longitudinal offset, check the
-              // overlap with the polygon including the next_lane(s) until the additional lanes get
-              // longer than ego vehicle length
-      if (!check_goal_footprint_inside_lanes(
-            next_lane, combined_lanelets, goal_footprint, next_lane_length)) {
-        next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
-        continue;
-      } else {
-        return true;
-      }
+    const auto next_lanelets = next_lanelets_up_to(
+      next_lane, up_to_distance - lanelet::geometry::length2d(next_lane), route_handler);
+    lanelets.insert(lanelets.end(), next_lanelets.begin(), next_lanelets.end());
+  }
+  return lanelets;
+}
+
+bool DefaultPlanner::check_goal_footprint_inside_lanes(
+  const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets,
+  const universe_utils::Polygon2d & goal_footprint) const
+{
+  universe_utils::MultiPolygon2d ego_lanes;
+  universe_utils::Polygon2d poly;
+  for (const auto & ll : path_lanelets) {
+    const auto left_shoulder = route_handler_.getLeftShoulderLanelet(ll);
+    if (left_shoulder) {
+      boost::geometry::convert(left_shoulder->polygon2d().basicPolygon(), poly);
+      ego_lanes.push_back(poly);
+    }
+    const auto right_shoulder = route_handler_.getRightShoulderLanelet(ll);
+    if (right_shoulder) {
+      boost::geometry::convert(right_shoulder->polygon2d().basicPolygon(), poly);
+      ego_lanes.push_back(poly);
     }
+    boost::geometry::convert(ll.polygon2d().basicPolygon(), poly);
+    ego_lanes.push_back(poly);
   }
-  return false;
+  const auto next_lanelets =
+    next_lanelets_up_to(current_lanelet, vehicle_info_.max_longitudinal_offset_m, route_handler_);
+  for (const auto & ll : next_lanelets) {
+    boost::geometry::convert(ll.polygon2d().basicPolygon(), poly);
+    ego_lanes.push_back(poly);
+  }
+
+  // check if goal footprint is in the ego lane
+  universe_utils::MultiPolygon2d difference;
+  boost::geometry::difference(goal_footprint, ego_lanes, difference);
+  return boost::geometry::is_empty(difference);
 }
 
 bool DefaultPlanner::is_goal_valid(
-  const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets)
+  const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets)
 {
   const auto logger = node_->get_logger();
 
@@ -337,16 +347,10 @@ bool DefaultPlanner::is_goal_valid(
   pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint));
   const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint);
 
-  double next_lane_length = 0.0;
-  // combine calculated route lanelets
-  const lanelet::ConstLanelet combined_prev_lanelet =
-    combine_lanelets_with_shoulder(path_lanelets, route_handler_);
-
   // check if goal footprint exceeds lane when the goal isn't in parking_lot
   if (
     param_.check_footprint_inside_lanes &&
-    !check_goal_footprint_inside_lanes(
-      closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
+    !check_goal_footprint_inside_lanes(closest_lanelet, path_lanelets, polygon_footprint) &&
     !is_in_parking_lot(
       lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()),
       lanelet::utils::conversion::toLaneletPoint(goal.position))) {
@@ -375,11 +379,7 @@ bool DefaultPlanner::is_goal_valid(
   // check if goal is in parking lot
   const auto parking_lots =
     lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr());
-  if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) {
-    return true;
-  }
-
-  return false;
+  return is_in_parking_lot(parking_lots, goal_lanelet_pt);
 }
 
 PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
@@ -429,7 +429,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
     return route_msg;
   }
 
-  if (route_handler_.isRouteLooped(route_sections)) {
+  if (route_handler::RouteHandler::isRouteLooped(route_sections)) {
     RCLCPP_WARN(logger, "Loop detected within route!");
     return route_msg;
   }

planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp

@@ -27,7 +27,6 @@
 #include <lanelet2_routing/RoutingGraph.h>
 #include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 
-#include <memory>
 #include <vector>
 
 namespace autoware::mission_planner::lanelet2
@@ -44,15 +43,17 @@ struct DefaultPlannerParameters
 class DefaultPlanner : public mission_planner::PlannerPlugin
 {
 public:
+  DefaultPlanner() : vehicle_info_(), is_graph_ready_(false), param_(), node_(nullptr) {}
+
   void initialize(rclcpp::Node * node) override;
   void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override;
-  bool ready() const override;
+  [[nodiscard]] bool ready() const override;
   LaneletRoute plan(const RoutePoints & points) override;
   void updateRoute(const PlannerPlugin::LaneletRoute & route) override;
   void clearRoute() override;
-  MarkerArray visualize(const LaneletRoute & route) const override;
-  MarkerArray visualize_debug_footprint(
-    autoware::universe_utils::LinearRing2d goal_footprint_) const;
+  [[nodiscard]] MarkerArray visualize(const LaneletRoute & route) const override;
+  [[nodiscard]] static MarkerArray visualize_debug_footprint(
+    autoware::universe_utils::LinearRing2d goal_footprint);
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
 
 private:
@@ -83,17 +84,16 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
    * @param next_lane_length the accumulated total length from the start lanelet of the search to
    * the lanelet of current goal query
    */
-  bool check_goal_footprint_inside_lanes(
-    const lanelet::ConstLanelet & current_lanelet,
-    const lanelet::ConstLanelet & combined_prev_lanelet,
-    const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length,
-    const double search_margin = 2.0);
+  [[nodiscard]] bool check_goal_footprint_inside_lanes(
+    const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets,
+    const universe_utils::Polygon2d & goal_footprint) const;
 
   /**
    * @brief return true if (1)the goal is in parking area or (2)the goal is on the lanes and the
    * footprint around the goal does not overlap the lanes
    */
-  bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets);
+  bool is_goal_valid(
+    const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets);
 
   /**
    * @brief project the specified goal pose onto the goal lanelet(the last preferred lanelet of

planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp

@@ -18,9 +18,6 @@
 
 #include <lanelet2_core/geometry/Lanelet.h>
 
-#include <unordered_set>
-#include <utility>
-
 autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon(
   autoware::universe_utils::LinearRing2d footprint)
 {
@@ -41,63 +38,6 @@ void insert_marker_array(
   a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
 }
 
-lanelet::ConstLanelet combine_lanelets_with_shoulder(
-  const lanelet::ConstLanelets & lanelets,
-  const autoware::route_handler::RouteHandler & route_handler)
-{
-  lanelet::Points3d lefts;
-  lanelet::Points3d rights;
-  lanelet::Points3d centers;
-  std::vector<uint64_t> left_bound_ids;
-  std::vector<uint64_t> right_bound_ids;
-
-  for (const auto & llt : lanelets) {
-    if (llt.id() != lanelet::InvalId) {
-      left_bound_ids.push_back(llt.leftBound().id());
-      right_bound_ids.push_back(llt.rightBound().id());
-    }
-  }
-
-  // lambda to add bound to target_bound
-  const auto add_bound = [](const auto & bound, auto & target_bound) {
-    std::transform(
-      bound.begin(), bound.end(), std::back_inserter(target_bound),
-      [](const auto & pt) { return lanelet::Point3d(pt); });
-  };
-  for (const auto & llt : lanelets) {
-    // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist
-    const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt);
-    if (left_shared_shoulder) {
-      // if exist, add left bound of SHOULDER lanelet to lefts
-      add_bound(left_shared_shoulder->leftBound(), lefts);
-    } else if (
-      // if not exist, add left bound of lanelet to lefts
-      // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`,
-      // then its left bound constitutes the left boundary of the entire merged lanelet
-      std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
-      add_bound(llt.leftBound(), lefts);
-    }
-
-    // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist
-    const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt);
-    if (right_shared_shoulder) {
-      // if exist, add right bound of SHOULDER lanelet to rights
-      add_bound(right_shared_shoulder->rightBound(), rights);
-    } else if (
-      // if not exist, add right bound of lanelet to rights
-      // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`,
-      // then its right bound constitutes the right boundary of the entire merged lanelet
-      std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
-      add_bound(llt.rightBound(), rights);
-    }
-  }
-
-  const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts);
-  const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights);
-  auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);
-  return std::move(combined_lanelet);
-}
-
 std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet)
 {
   std::vector<geometry_msgs::msg::Point> centerline_points;