Merge branch 'refactor/start_planner_filtering' into refactor/use_run…

…_base_class_function

Signed-off-by: kyoichi-sugahara <[email protected]>

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

@@ -168,6 +168,7 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0)
   }
 
   T dst;
+  dst.reserve(points.size());
 
   for (size_t i = 0; i <= start_idx; ++i) {
     dst.push_back(points.at(i));

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

@@ -0,0 +1,77 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_utils/trajectory/trajectory.hpp"
+
+#include <gtest/gtest.h>
+#include <gtest/internal/gtest-port.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <random>
+
+namespace
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using tier4_autoware_utils::createPoint;
+using tier4_autoware_utils::createQuaternionFromRPY;
+
+constexpr double epsilon = 1e-6;
+
+geometry_msgs::msg::Pose createPose(
+  double x, double y, double z, double roll, double pitch, double yaw)
+{
+  geometry_msgs::msg::Pose p;
+  p.position = createPoint(x, y, z);
+  p.orientation = createQuaternionFromRPY(roll, pitch, yaw);
+  return p;
+}
+
+template <class T>
+T generateTestTrajectory(
+  const size_t num_points, const double point_interval, const double vel = 0.0,
+  const double init_theta = 0.0, const double delta_theta = 0.0)
+{
+  using Point = typename T::_points_type::value_type;
+
+  T traj;
+  for (size_t i = 0; i < num_points; ++i) {
+    const double theta = init_theta + i * delta_theta;
+    const double x = i * point_interval * std::cos(theta);
+    const double y = i * point_interval * std::sin(theta);
+
+    Point p;
+    p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta);
+    p.longitudinal_velocity_mps = vel;
+    traj.points.push_back(p);
+  }
+
+  return traj;
+}
+}  // namespace
+
+TEST(trajectory_benchmark, DISABLED_calcLateralOffset)
+{
+  std::random_device r;
+  std::default_random_engine e1(r());
+  std::uniform_real_distribution<double> uniform_dist(0.0, 1000.0);
+
+  using motion_utils::calcLateralOffset;
+
+  const auto traj = generateTestTrajectory<Trajectory>(1000, 1.0, 0.0, 0.0, 0.1);
+  constexpr auto nb_iteration = 10000;
+  for (auto i = 0; i < nb_iteration; ++i) {
+    const auto point = createPoint(uniform_dist(e1), uniform_dist(e1), 0.0);
+    calcLateralOffset(traj.points, point);
+  }
+}

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -127,8 +127,8 @@
           <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
           <remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
           <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
-          <remap from="~/output/max_velocity" to="/planning/scenario_planning/max_velocity_candidates"/>
-          <remap from="~/output/velocity_limit_clear_command" to="/planning/scenario_planning/clear_velocity_limit"/>
+          <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)"/>

perception/image_projection_based_fusion/CMakeLists.txt

@@ -146,6 +146,10 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
     EXECUTABLE pointpainting_fusion_node
   )
 
+  install(
+    TARGETS pointpainting_cuda_lib
+    DESTINATION lib
+  )
 else()
   message("Skipping build of some nodes due to missing dependencies")
 endif()

planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md

@@ -252,7 +252,7 @@ If a safe path cannot be generated from the current position, search backwards f
 | max_back_distance             | [m]  | double | maximum back distance                                                                                                                                                | 30.0           |
 | backward_search_resolution    | [m]  | double | distance interval for searching backward pull out start point                                                                                                        | 2.0            |
 | backward_path_update_duration | [s]  | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out                                                | 3.0            |
-| ignore_distance_from_lane_end | [m]  | double | distance from end of pull out lanes for ignoring start candidates                                                                                                    | 15.0           |
+| ignore_distance_from_lane_end | [m]  | double | If distance from shift start pose to end of shoulder lane is less than this value, this start pose candidate is ignored                                              | 15.0           |
 
 ### **freespace pull out**
 

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -802,6 +802,8 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
   const auto & stop_objects_in_pull_out_lanes =
     filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity);
 
+  // Set the maximum backward distance less than the distance from the vehicle's base_link to the
+  // lane's rearmost point to prevent lane departure.
   const double current_arc_length =
     lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length;
   const double allowed_backward_distance = std::clamp(
@@ -843,13 +845,13 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
 PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
   const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const
 {
+  const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
+    *planner_data_->dynamic_object, velocity_threshold);
+
   // filter for objects located in pull_out_lanes and moving at a speed below the threshold
-  const auto [objects_in_pull_out_lanes, others] =
+  const auto [stop_objects_in_pull_out_lanes, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *planner_data_->dynamic_object, pull_out_lanes,
-      utils::path_safety_checker::isPolygonOverlapLanelet);
-  const auto stop_objects_in_pull_out_lanes = utils::path_safety_checker::filterObjectsByVelocity(
-    objects_in_pull_out_lanes, velocity_threshold);
+      stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
 
   return stop_objects_in_pull_out_lanes;
 }

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -288,7 +288,7 @@ bool DefaultPlanner::check_goal_footprint(
     lanelet::ConstLanelets lanelets;
     lanelets.push_back(combined_prev_lanelet);
     lanelets.push_back(next_lane);
-    lanelet::ConstLanelet combined_lanelets = lanelet::utils::combineLaneletsShape(lanelets);
+    lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets);
 
     // if next lanelet length longer than vehicle longitudinal offset
     if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
@@ -347,7 +347,7 @@ bool DefaultPlanner::is_goal_valid(
 
   double next_lane_length = 0.0;
   // combine calculated route lanelets
-  lanelet::ConstLanelet combined_prev_lanelet = lanelet::utils::combineLaneletsShape(path_lanelets);
+  lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets);
 
   // check if goal footprint exceeds lane when the goal isn't in parking_lot
   if (

planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

@@ -54,6 +54,39 @@ void insert_marker_array(
   a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
 }
 
+lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets)
+{
+  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() != 0) {
+      left_bound_ids.push_back(llt.leftBound().id());
+      right_bound_ids.push_back(llt.rightBound().id());
+    }
+  }
+
+  for (const auto & llt : lanelets) {
+    if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
+      for (const auto & pt : llt.leftBound()) {
+        lefts.push_back(lanelet::Point3d(pt));
+      }
+    }
+    if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
+      for (const auto & pt : llt.rightBound()) {
+        rights.push_back(lanelet::Point3d(pt));
+      }
+    }
+  }
+  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;

planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp

@@ -49,6 +49,7 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub
 void insert_marker_array(
   visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);
 
+lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets);
 std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
 geometry_msgs::msg::Pose convertBasicPoint3dToPose(
   const lanelet::BasicPoint3d & point, const double lane_yaw);