Merge branch 'beta/v0.3.18' into feat/planning/upd_obstacle_hunting

.github/workflows/dispatch-push-event.yaml

@@ -0,0 +1,45 @@
+name: dispatch-push-event
+on:
+  push:
+
+jobs:
+  search-dispatch-repo:
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        include:
+          - { version: beta/v0.3.**, dispatch-repo: pilot-auto.x1.eve }
+    outputs:
+      dispatch-repo: ${{ steps.search-dispatch-repo.outputs.value }}
+    steps:
+      - name: Search dispatch repo
+        id: search-dispatch-repo
+        run: |
+          if [[ ${{ github.ref_name }} =~ ${{ matrix.version }} ]]; then
+            echo ::set-output name=value::"${{ matrix.dispatch-repo }}"
+            echo "Detected beta branch: ${{ github.ref_name }}"
+            echo "Dispatch repository: ${{ matrix.dispatch-repo }}"
+          fi
+
+  dispatch-push-event:
+    runs-on: ubuntu-latest
+    needs: search-dispatch-repo
+    if: ${{ needs.search-dispatch-repo.outputs.dispatch-repo != '' }}
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v1
+        with:
+          app_id: ${{ secrets.INTERNAL_APP_ID }}
+          private_key: ${{ secrets.INTERNAL_PRIVATE_KEY }}
+
+      # 注意: workflow_dispatchで指定するブランチはmain固定となっているため、dispatch-repoのmainブランチにupdate-beta-branch.yamlが存在することが前提条件。
+      - name: Dispatch the update-beta-branch workflow
+        run: |
+          curl -L \
+          -X POST \
+          -H "Accept: application/vnd.github+json" \
+          -H "Authorization: Bearer ${{ steps.generate-token.outputs.token }}" \
+          -H "X-GitHub-Api-Version: 2022-11-28" \
+          https://api.github.com/repos/tier4/${{ needs.search-dispatch-repo.outputs.dispatch-repo }}/actions/workflows/update-beta-branch.yaml/dispatches \
+          -d '{"ref":"main"}'

common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp

@@ -38,60 +38,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
 
 namespace tf2
 {
-
-
-/*************/
-/** Point32 **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The point to transform, as a Point32 message.
- * \param t_out The transformed point, as a Point32 message.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template<>
-inline
-void doTransform(
-  const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
-  t_out.x = static_cast<float>(v_out[0]);
-  t_out.y = static_cast<float>(v_out[1]);
-  t_out.z = static_cast<float>(v_out[2]);
-}
-
-
-/*************/
-/** Polygon **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The polygon to transform.
- * \param t_out The transformed polygon.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template<>
-inline
-void doTransform(
-  const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  // Don't call the Point32 doTransform to avoid doing this conversion every time
-  const auto kdl_frame = gmTransformToKDL(transform);
-  // We don't use std::back_inserter to allow aliasing between t_in and t_out
-  t_out.points.resize(t_in.points.size());
-  for (size_t i = 0; i < t_in.points.size(); ++i) {
-    const KDL::Vector v_out = kdl_frame * KDL::Vector(
-      t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
-    t_out.points[i].x = static_cast<float>(v_out[0]);
-    t_out.points[i].y = static_cast<float>(v_out[1]);
-    t_out.points[i].z = static_cast<float>(v_out[2]);
-  }
-}
-
 /******************/
 /** Quaternion32 **/
 /******************/

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml

@@ -14,3 +14,4 @@
     drivable_area_margin: 6.0
     refine_goal_search_radius_range: 7.5
     intersection_search_distance: 30.0
+    path_interval: 2.0

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -17,4 +17,6 @@
     refine_goal_search_radius_range: 7.5
     intersection_search_distance: 30.0
 
+    path_interval: 2.0
+
     visualize_drivable_area_for_shared_linestrings_lanelet: true

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -50,6 +50,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner
@@ -136,7 +137,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief extract path from behavior tree output
    */
-  PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out);
+  std::pair<PathWithLaneId::SharedPtr, PathWithLaneId::SharedPtr> getPath(
+    const BehaviorModuleOutput & bt_out);
 
   /**
    * @brief extract path candidate from behavior tree output

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -37,6 +37,8 @@ struct BehaviorPathPlannerParameters
   double turn_light_on_threshold_dis_long;
   double turn_light_on_threshold_time;
 
+  double path_interval;
+
   // vehicle info
   double wheel_base;
   double front_overhang;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -165,6 +165,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.turn_light_on_threshold_dis_lat = declare_parameter("turn_light_on_threshold_dis_lat", 0.3);
   p.turn_light_on_threshold_dis_long = declare_parameter("turn_light_on_threshold_dis_long", 10.0);
   p.turn_light_on_threshold_time = declare_parameter("turn_light_on_threshold_time", 3.0);
+  p.path_interval = declare_parameter<double>("path_interval");
   p.visualize_drivable_area_for_shared_linestrings_lanelet =
     declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true);
 
@@ -477,7 +478,10 @@ void BehaviorPathPlannerNode::run()
   const auto output = bt_manager_->run(planner_data_);
 
   // path handling
-  const auto path = getPath(output);
+  const auto paths = getPath(output);
+  const auto path = paths.first;
+  const auto original_path = paths.second;
+
   const auto path_candidate = getPathCandidate(output);
   planner_data_->prev_output_path = path;
 
@@ -503,7 +507,7 @@ void BehaviorPathPlannerNode::run()
       hazard_signal.command = output.turn_signal_info.hazard_signal.command;
     } else {
       turn_signal = turn_signal_decider_.getTurnSignal(
-        *path, planner_data_->self_pose->pose, *(planner_data_->route_handler),
+        *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler),
         output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance);
       hazard_signal.command = HazardLightsCommand::DISABLE;
     }
@@ -526,7 +530,9 @@ void BehaviorPathPlannerNode::run()
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
-PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output)
+// output: spline interpolated path, original path
+std::pair<PathWithLaneId::SharedPtr, PathWithLaneId::SharedPtr> BehaviorPathPlannerNode::getPath(
+  const BehaviorModuleOutput & bt_output)
 {
   // TODO(Horibe) do some error handling when path is not available.
 
@@ -535,7 +541,10 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO
   path->header.stamp = this->now();
   RCLCPP_DEBUG(
     get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND");
-  return path;
+
+  const auto resampled_path =
+    util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval);
+  return std::make_pair(std::make_shared<PathWithLaneId>(resampled_path), path);
 }
 
 PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(

planning/behavior_path_planner/src/path_utilities.cpp

@@ -90,7 +90,18 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end)
 PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval)
 {
   const auto base_points = calcPathArcLengthArray(path);
-  const auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval);
+  auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval);
+
+  constexpr double epsilon = 0.01;
+  for (const auto & b : base_points) {
+    const auto is_almost_same_value = std::find_if(
+      sampling_points.begin(), sampling_points.end(),
+      [&](const auto v) { return std::abs(v - b) < epsilon; });
+    if (is_almost_same_value == sampling_points.end()) {
+      sampling_points.push_back(b);
+    }
+  }
+  std::sort(sampling_points.begin(), sampling_points.end());
 
   if (base_points.empty() || sampling_points.empty()) {
     return path;
@@ -122,22 +133,25 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv
 
   // For LaneIds, Type, Twist
   //
-  // ------|----|----|----|----|----|----|-------> resampled
-  //      [0]  [1]  [2]  [3]  [4]  [5]  [6]
+  // ------|----|----|----|----|----|----|----|--------> resampled
+  //      [0]  [1]  [2]  [3]  [4]  [5]  [6]  [7]
   //
-  // --------|---------------|----------|---------> base
-  //        [0]             [1]        [2]
+  // ------|---------------|----------|-------|---> base
+  //      [0]             [1]        [2]     [3]
   //
-  // resampled[0~3] = base[0]
-  // resampled[4~5] = base[1]
-  // resampled[6] = base[2]
+  // resampled[0] = base[0]
+  // resampled[1~3] = base[1]
+  // resampled[4~5] = base[2]
+  // resampled[6~7] = base[3]
   //
   size_t base_idx{0};
   for (size_t i = 0; i < resampled_path.points.size(); ++i) {
-    while (base_idx < base_points.size() - 1 && sampling_points.at(i) > base_points.at(base_idx)) {
+    while (base_idx < base_points.size() - 1 &&
+           ((sampling_points.at(i) > base_points.at(base_idx)) ||
+            (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) {
       ++base_idx;
     }
-    size_t ref_idx = std::max(static_cast<int>(base_idx) - 1, 0);
+    size_t ref_idx = std::max(static_cast<int>(base_idx), 0);
     if (i == resampled_path.points.size() - 1) {
       ref_idx = base_points.size() - 1;  // for last point
     }

planning/behavior_path_planner/src/utilities.cpp

@@ -26,29 +26,6 @@
 #include <string>
 #include <vector>
 
-namespace tier4_autoware_utils
-{
-template <class T>
-double calcLateralOffset(
-  const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx)
-{
-  validateNonEmpty(points);
-
-  const auto p_front = getPoint(points.at(seg_idx));
-  const auto p_back = getPoint(points.at(seg_idx + 1));
-
-  const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0};
-  const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0};
-
-  if (segment_vec.norm() == 0.0) {
-    throw std::runtime_error("Same points are given.");
-  }
-
-  const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec);
-  return cross_vec(2) / segment_vec.norm();
-}
-}  // namespace tier4_autoware_utils
-
 namespace drivable_area_utils
 {
 double quantize(const double val, const double resolution)

planning/behavior_velocity_planner/src/node.cpp

@@ -420,10 +420,10 @@ void BehaviorVelocityPlannerNode::onTrigger(
   const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path));
 
   // interpolation
-  const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_);
+  // const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_);
 
   // check stop point
-  auto output_path_msg = filterStopPathPoint(interpolated_path_msg);
+  auto output_path_msg = filterStopPathPoint(filtered_path);
   output_path_msg.header.frame_id = "map";
   output_path_msg.header.stamp = this->now();
 

planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp

@@ -357,8 +357,13 @@ bool DetectionAreaModule::isOverLine(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const
 {
-  return tier4_autoware_utils::calcSignedArcLength(
-           path.points, self_pose.position, line_pose.position) < 0;
+  const PointWithSearchRangeIndex src_point_with_search_range_index =
+    planning_utils::findFirstNearSearchRangeIndex(path.points, self_pose.position);
+  const PointWithSearchRangeIndex dst_point_with_search_range_index = {
+    line_pose.position, planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_)};
+
+  return planning_utils::calcSignedArcLengthWithSearchIndex(
+           path.points, src_point_with_search_range_index, dst_point_with_search_range_index) < 0;
 }
 
 bool DetectionAreaModule::hasEnoughBrakingDistance(

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -53,6 +53,10 @@ template <typename T>
 boost::optional<geometry_msgs::msg::Pose> lerpPose(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return {};
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =
@@ -94,6 +98,10 @@ template <typename T>
 double lerpTwistX(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return 0.0;
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =
@@ -116,6 +124,10 @@ template <typename T>
 double lerpPoseZ(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return 0.0;
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =

planning/obstacle_avoidance_planner/src/node.cpp

@@ -1093,6 +1093,9 @@ void ObstacleAvoidancePlanner::calcVelocity(
   const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points) const
 {
+  if (path_points.size() < 2) {
+    return;
+  }
   for (size_t i = 0; i < traj_points.size(); i++) {
     const size_t nearest_seg_idx = [&]() {
       const auto opt_seg_idx = tier4_autoware_utils::findNearestSegmentIndex(

planning/obstacle_stop_planner/src/node.cpp

@@ -798,6 +798,10 @@ void ObstacleStopPlannerNode::insertVelocity(
   const VehicleInfo & vehicle_info, const double current_acc, const double current_vel,
   const StopParam & stop_param)
 {
+  if (output.size() < 3) {
+    return;
+  }
+
   if (planner_data.stop_require) {
     // insert stop point
     const auto traj_end_idx = output.size() - 1;

planning/route_handler/src/route_handler.cpp

@@ -121,7 +121,7 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path)
       continue;
     }
 
-    constexpr double min_dist = 0.001;
+    constexpr double min_dist = 0.1;
     if (
       tier4_autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) <
       min_dist) {