fix(behavior_path_planner): prevent crashes with the drivable area expansion

.cspell-partial.json

@@ -1,5 +1,9 @@
 {
-  "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"],
+  "ignorePaths": [
+    "**/perception/**",
+    "sensing/tier4_pcl_extensions/include/**",
+    "perception/bytetrack/lib/**"
+  ],
   "ignoreRegExpList": [],
   "words": []
 }

.github/CODEOWNERS

@@ -14,6 +14,7 @@ common/component_interface_tools/** [email protected] [email protected] m
 common/component_interface_utils/** [email protected] [email protected] [email protected] [email protected]
 common/cuda_utils/** [email protected] [email protected]
 common/fake_test_node/** [email protected] [email protected] [email protected] [email protected]
+common/geography_utils/** [email protected]
 common/global_parameter_loader/** [email protected]
 common/glog_component/** [email protected]
 common/goal_distance_calculator/** [email protected]
@@ -117,7 +118,7 @@ perception/lidar_apollo_segmentation_tvm/** [email protected] xinyu.wang@
 perception/lidar_apollo_segmentation_tvm_nodes/** [email protected] [email protected]
 perception/lidar_centerpoint/** [email protected] [email protected]
 perception/lidar_centerpoint_tvm/** [email protected] [email protected]
-perception/map_based_prediction/** [email protected] [email protected] [email protected] [email protected]
+perception/map_based_prediction/** [email protected] shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp [email protected] [email protected] [email protected]
 perception/multi_object_tracker/** [email protected] [email protected]
 perception/object_merger/** [email protected] [email protected]
 perception/object_range_splitter/** [email protected]
@@ -166,7 +167,7 @@ planning/freespace_planning_algorithms/** [email protected] takamasa.hori
 planning/mission_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/motion_velocity_smoother/** [email protected] [email protected] [email protected] [email protected]
 planning/obstacle_avoidance_planner/** [email protected] [email protected] [email protected]
-planning/obstacle_cruise_planner/** [email protected] [email protected]
+planning/obstacle_cruise_planner/** [email protected] [email protected] [email protected] [email protected]
 planning/obstacle_stop_planner/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/obstacle_velocity_limiter/** [email protected]
 planning/path_smoother/** [email protected] [email protected]

.github/workflows/beta-to-tier4-main-sync.yaml

@@ -0,0 +1,34 @@
+name: beta-to-tier4-main-sync
+
+on:
+  workflow_dispatch:
+    inputs:
+      source_branch:
+        description: Source branch
+        required: true
+        type: string
+
+jobs:
+  sync-beta-branch:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v1
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run sync-branches
+        uses: autowarefoundation/autoware-github-actions/sync-branches@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          base-branch: tier4/main
+          sync-pr-branch: beta-to-tier4-main-sync
+          sync-target-repository: https://github.com/tier4/autoware.universe.git
+          sync-target-branch: ${{ inputs.source_branch }}
+          pr-title: "chore: sync beta branch ${{ inputs.source_branch }} with tier4/main"
+          pr-labels: |
+            bot
+            sync-beta-branch
+          auto-merge-method: merge

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

.github/workflows/sync-upstream.yaml

@@ -1,9 +1,12 @@
 name: sync-upstream
 
 on:
-  schedule:
-    - cron: 0 1 * * 1-5
   workflow_dispatch:
+    inputs:
+      target_branch:
+        description: Target branch
+        required: true
+        type: string
 
 jobs:
   sync-upstream:
@@ -28,12 +31,17 @@ jobs:
           script: |
             const holiday_jp = require(`${process.env.GITHUB_WORKSPACE}/node_modules/@holiday-jp/holiday_jp`)
             core.setOutput('holiday', holiday_jp.isHoliday(new Date()));
+      - name: Print warning for invalid branch name
+        if: ${{ inputs.target_branch  == 'tier4/main' }}
+        run: |
+          echo This action cannot be performed on 'tier4/main' branch
+
       - name: Run sync-branches
+        if: ${{ inputs.target_branch  != 'tier4/main' }}
         uses: autowarefoundation/autoware-github-actions/sync-branches@v1
-        if: ${{ steps.is-holiday.outputs.holiday != 'true' || github.event_name == 'workflow_dispatch' }}
         with:
           token: ${{ steps.generate-token.outputs.token }}
-          base-branch: tier4/main
+          base-branch: ${{ inputs.target_branch }}
           sync-pr-branch: sync-upstream
           sync-target-repository: https://github.com/autowarefoundation/autoware.universe.git
           sync-target-branch: main

common/geography_utils/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.14)
+project(geography_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# GeographicLib
+find_package(PkgConfig)
+find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h
+  PATH_SUFFIXES GeographicLib
+)
+set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR})
+find_library(GeographicLib_LIBRARIES NAMES Geographic)
+
+ament_auto_add_library(geography_utils SHARED
+  src/height.cpp
+  src/projection.cpp
+  src/lanelet2_projector.cpp
+)
+
+target_link_libraries(geography_utils
+  ${GeographicLib_LIBRARIES}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+
+  file(GLOB_RECURSE test_files test/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_geography_utils ${test_files})
+
+  target_link_libraries(test_geography_utils
+  geography_utils
+  )
+endif()
+
+ament_auto_package()

common/geography_utils/README.md

@@ -0,0 +1,5 @@
+# geography_utils
+
+## Purpose
+
+This package contains geography-related functions used by other packages, so please refer to them as needed.

common/geography_utils/include/geography_utils/height.hpp

@@ -0,0 +1,33 @@
+// 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.
+
+#ifndef GEOGRAPHY_UTILS__HEIGHT_HPP_
+#define GEOGRAPHY_UTILS__HEIGHT_HPP_
+
+#include <string>
+
+namespace geography_utils
+{
+
+typedef double (*HeightConversionFunction)(
+  const double height, const double latitude, const double longitude);
+double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude);
+double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude);
+double convert_height(
+  const double height, const double latitude, const double longitude,
+  const std::string & source_vertical_datum, const std::string & target_vertical_datum);
+
+}  // namespace geography_utils
+
+#endif  // GEOGRAPHY_UTILS__HEIGHT_HPP_

common/geography_utils/include/geography_utils/lanelet2_projector.hpp

@@ -0,0 +1,32 @@
+// 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.
+
+#ifndef GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
+#define GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
+
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
+
+#include <lanelet2_io/Projection.h>
+
+#include <memory>
+
+namespace geography_utils
+{
+using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo;
+
+std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInfo & projector_info);
+
+}  // namespace geography_utils
+
+#endif  // GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_

common/geography_utils/include/geography_utils/projection.hpp

@@ -0,0 +1,33 @@
+// 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.
+
+#ifndef GEOGRAPHY_UTILS__PROJECTION_HPP_
+#define GEOGRAPHY_UTILS__PROJECTION_HPP_
+
+#include <geographic_msgs/msg/geo_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
+
+namespace geography_utils
+{
+using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo;
+using GeoPoint = geographic_msgs::msg::GeoPoint;
+using LocalPoint = geometry_msgs::msg::Point;
+
+LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info);
+GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info);
+
+}  // namespace geography_utils
+
+#endif  // GEOGRAPHY_UTILS__PROJECTION_HPP_

common/geography_utils/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>geography_utils</name>
+  <version>0.1.0</version>
+  <description>The geography_utils package</description>
+  <maintainer email="[email protected]">Koji Minoda</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>geographic_msgs</depend>
+  <depend>geographiclib</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>lanelet2_io</depend>
+  <depend>tier4_map_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/geography_utils/src/height.cpp

@@ -0,0 +1,63 @@
+// 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 <GeographicLib/Geoid.hpp>
+#include <geography_utils/height.hpp>
+
+#include <map>
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+namespace geography_utils
+{
+
+double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude)
+{
+  GeographicLib::Geoid egm2008("egm2008-1");
+  // cSpell: ignore ELLIPSOIDTOGEOID
+  return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID);
+}
+
+double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude)
+{
+  GeographicLib::Geoid egm2008("egm2008-1");
+  // cSpell: ignore GEOIDTOELLIPSOID
+  return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID);
+}
+
+double convert_height(
+  const double height, const double latitude, const double longitude,
+  const std::string & source_vertical_datum, const std::string & target_vertical_datum)
+{
+  if (source_vertical_datum == target_vertical_datum) {
+    return height;
+  }
+  std::map<std::pair<std::string, std::string>, HeightConversionFunction> conversion_map;
+  conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008;
+  conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84;
+
+  auto key = std::make_pair(source_vertical_datum, target_vertical_datum);
+  if (conversion_map.find(key) != conversion_map.end()) {
+    return conversion_map[key](height, latitude, longitude);
+  } else {
+    std::string error_message =
+      "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " +
+      std::string(target_vertical_datum.c_str());
+
+    throw std::invalid_argument(error_message);
+  }
+}
+
+}  // namespace geography_utils