Merge branch 'main' into feat/define_util_function_for_start_goal_pla…

…nner

common/geography_utils/CMakeLists.txt

@@ -0,0 +1,35 @@
+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
+)
+
+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,36 @@
+// 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 <map>
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+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/package.xml

@@ -0,0 +1,22 @@
+<?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>geographiclib</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

common/geography_utils/test/test_height.cpp

@@ -0,0 +1,92 @@
+// 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 <geography_utils/height.hpp>
+
+#include <gtest/gtest.h>
+
+#include <stdexcept>
+#include <string>
+
+// Test case to verify if same source and target datums return original height
+TEST(Tier4GeographyUtils, SameSourceTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+  const std::string datum = "WGS84";
+
+  double converted_height =
+    geography_utils::convert_height(height, latitude, longitude, datum, datum);
+
+  EXPECT_DOUBLE_EQ(height, converted_height);
+}
+
+// Test case to verify valid source and target datums
+TEST(Tier4GeographyUtils, ValidSourceTargetDatum)
+{
+  // Calculated with
+  // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+  const double target_height = -30.18;
+
+  double converted_height =
+    geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008");
+
+  EXPECT_NEAR(target_height, converted_height, 0.1);
+}
+
+// Test case to verify invalid source and target datums
+TEST(Tier4GeographyUtils, InvalidSourceTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"),
+    std::invalid_argument);
+}
+
+// Test case to verify invalid source datums
+TEST(Tier4GeographyUtils, InvalidSourceDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"),
+    std::invalid_argument);
+}
+
+// Test case to verify invalid target datums
+TEST(Tier4GeographyUtils, InvalidTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"),
+    std::invalid_argument);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md

@@ -55,12 +55,12 @@ where `common_param` is vehicle common parameter, which defines vehicle common m
 The `longitudinal_acceleration_resolution` is determine by the following
 
 ```C++
-longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_deceleration) / longitudinal_acceleration_sampling_num
+longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_acceleration) / longitudinal_acceleration_sampling_num
 ```
 
 Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, longitudinal acceleration becomes positive value (not decelerate).
 
-The following figure illustrates when `lane_change_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`.
+The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`.
 
 ![path_samples](../image/lane_change/lane_change-candidate_path_samples.png)
 
@@ -79,12 +79,12 @@ The maximum and minimum lateral accelerations are defined in the lane change par
 | 4.0          | 0.3                          | 0.4                          |
 | 6.0          | 0.3                          | 0.5                          |
 
-In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.2 and 0.35 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values.
+In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.25 and 0.4 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values.
 
 Within this range, we sample the lateral acceleration for the ego vehicle. Similar to the method used for sampling longitudinal acceleration, the resolution of lateral acceleration (lateral_acceleration_resolution) is determined by the following:
 
 ```C++
-lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_deceleration) / lateral_acceleration_sampling_num
+lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_acceleration) / lateral_acceleration_sampling_num
 ```
 
 #### Candidate Path's validity check
@@ -280,7 +280,6 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `object_shiftable_ratio_threshold`          | [-]    | double  | Vehicles around the center line within this distance ratio will be excluded from parking objects                | 0.6                |
 | `min_length_for_turn_signal_activation`     | [m]    | double  | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length      | 10.0               |
 | `length_ratio_for_turn_signal_deactivation` | [-]    | double  | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8                |
-| `object_shiftable_ratio_threshold`          | [-]    | double  | Vehicles around the center line within this distance ratio will be excluded from parking objects                | 0.6                |
 | `max_longitudinal_acc`                      | [-]    | double  | maximum longitudinal acceleration for lane change                                                               | 1.0                |
 | `min_longitudinal_acc`                      | [-]    | double  | maximum longitudinal deceleration for lane change                                                               | -1.0               |
 | `lateral_acceleration.velocity`             | [m/s]  | double  | Reference velocity for lateral acceleration calculation (look up table)                                         | [0.0, 4.0, 10.0]   |

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -74,7 +74,6 @@ struct DynamicAvoidanceParameters
   double max_oncoming_crossing_object_angle{0.0};
 
   // drivable area generation
-  std::string polygon_generation_method{};
   double min_obj_path_based_lon_polygon_margin{0.0};
   double lat_offset_from_obstacle{0.0};
   double max_lat_offset_to_avoid{0.0};
@@ -94,6 +93,10 @@ struct DynamicAvoidanceParameters
 class DynamicAvoidanceModule : public SceneModuleInterface
 {
 public:
+  enum class PolygonGenerationMethod {
+    EGO_PATH_BASE = 0,
+    OBJECT_PATH_BASE,
+  };
   struct DynamicAvoidanceObject
   {
     DynamicAvoidanceObject(
@@ -128,15 +131,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     std::optional<MinMaxValue> lat_offset_to_avoid{std::nullopt};
     bool is_collision_left;
     bool should_be_avoided{false};
+    PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::OBJECT_PATH_BASE};
 
     void update(
       const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
-      const bool arg_is_collision_left, const bool arg_should_be_avoided)
+      const bool arg_is_collision_left, const bool arg_should_be_avoided,
+      const PolygonGenerationMethod & arg_polygon_generation_method)
     {
       lon_offset_to_avoid = arg_lon_offset_to_avoid;
       lat_offset_to_avoid = arg_lat_offset_to_avoid;
       is_collision_left = arg_is_collision_left;
       should_be_avoided = arg_should_be_avoided;
+      polygon_generation_method = arg_polygon_generation_method;
     }
   };
 
@@ -230,11 +236,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     void updateObject(
       const std::string & uuid, const MinMaxValue & lon_offset_to_avoid,
       const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left,
-      const bool should_be_avoided)
+      const bool should_be_avoided, const PolygonGenerationMethod & polygon_generation_method)
     {
       if (object_map_.count(uuid) != 0) {
         object_map_.at(uuid).update(
-          lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided);
+          lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided,
+          polygon_generation_method);
       }
     }
 
@@ -312,7 +319,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface
   void updateTargetObjects();
   bool willObjectCutIn(
     const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,
-    const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const;
+    const double obj_tangent_vel, const LatLonOffset & lat_lon_offset,
+    PolygonGenerationMethod & polygon_generation_method) const;
   DecisionWithReason willObjectCutOut(
     const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left,
     const std::optional<DynamicAvoidanceObject> & prev_object) const;