fix: virtual traffic light/suppress launch

.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"}'

.pre-commit-config.yaml

@@ -35,8 +35,9 @@ repos:
       - id: yamllint
 
   - repo: https://github.com/tier4/pre-commit-hooks-ros
-    rev: v0.6.0
+    rev: v0.8.0
     hooks:
+      - id: flake8-ros
       - id: prettier-xacro
       - id: prettier-launch-xml
       - id: prettier-package-xml
@@ -65,22 +66,6 @@ repos:
       - id: black
         args: [--line-length=100]
 
-  - repo: https://github.com/PyCQA/flake8
-    rev: 4.0.1
-    hooks:
-      - id: flake8
-        additional_dependencies:
-          [
-            flake8-blind-except,
-            flake8-builtins,
-            flake8-class-newline,
-            flake8-comprehensions,
-            flake8-deprecated,
-            flake8-docstrings,
-            flake8-import-order,
-            flake8-quotes,
-          ]
-
   - repo: https://github.com/pre-commit/mirrors-clang-format
     rev: v13.0.1
     hooks:

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 **/
 /******************/

common/tier4_debug_tools/package.xml

@@ -17,8 +17,8 @@
   <depend>tier4_debug_msgs</depend>
 
   <exec_depend>launch_ros</exec_depend>
-  <exec_depend>rclpy</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-rtree</exec_depend>
+  <exec_depend>rclpy</exec_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

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

perception/elevation_map_loader/src/elevation_map_loader_node.cpp

@@ -102,7 +102,8 @@ void ElevationMapLoaderNode::publish()
     try {
       is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag(
         *data_manager_.elevation_map_path_, "elevation_map", elevation_map_);
-    } catch (rosbag2_storage_plugins::SqliteException & e) {
+    } catch (const std::runtime_error & e) {
+      RCLCPP_ERROR(this->get_logger(), e.what());
       is_bag_loaded = false;
     }
     if (!is_bag_loaded) {

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/include/scene_module/detection_area/scene.hpp

@@ -62,7 +62,8 @@ class DetectionAreaModule : public SceneModuleInterface
 
 public:
   DetectionAreaModule(
-    const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem,
+    const int64_t module_id, const size_t lane_id,
+    const lanelet::autoware::DetectionArea & detection_area_reg_elem,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
@@ -106,6 +107,8 @@ class DetectionAreaModule : public SceneModuleInterface
 
   // Debug
   DebugData debug_data_;
+
+  int64_t lane_id_;
 };
 }  // namespace behavior_velocity_planner
 

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();