From 741ab23abfdf0b62f6c8848feb9c8d90ab696753 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 30 Aug 2023 12:03:51 +0200 Subject: [PATCH 1/6] build(behavior_velocity_intersection_module): add libopencv-dev dependency (#4814) Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_intersection_module/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 1d6067d4be322..f22e9d788fc65 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -23,6 +23,7 @@ geometry_msgs interpolation lanelet2_extension + libopencv-dev magic_enum motion_utils nav_msgs From 812910abf1c8d21de0337d64cd29835ca8c400d1 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 30 Aug 2023 19:48:12 +0900 Subject: [PATCH 2/6] feat(geography_utils): add geography_utils package (#4807) * first commit Signed-off-by: kminoda * update Signed-off-by: kminoda * style(pre-commit): autofix * update height.cpp Signed-off-by: kminoda * style(pre-commit): autofix * updat Signed-off-by: kminoda * style(pre-commit): autofix * add test Signed-off-by: kminoda * revert tier4_autoware_utils Signed-off-by: kminoda * style(pre-commit): autofix * fix cspell Signed-off-by: kminoda * remove and revert convert.cpp Signed-off-by: kminoda * remove boost Signed-off-by: kminoda * update comment Signed-off-by: kminoda * remove maybe_unsued Signed-off-by: kminoda * style(pre-commit): autofix * rename from tier4_geography_utils to geography_utils Signed-off-by: kminoda * rename from tier4_geography_utils to geography_utils Signed-off-by: kminoda * style(pre-commit): autofix * add some test Signed-off-by: kminoda * style(pre-commit): autofix * edit test Signed-off-by: kminoda * rename namespace Signed-off-by: kminoda * style(pre-commit): autofix * rename namespace complete Signed-off-by: kminoda * style(pre-commit): autofix * use angle brackets inclusion Signed-off-by: kminoda * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/geography_utils/CMakeLists.txt | 35 +++++++ common/geography_utils/README.md | 5 + .../include/geography_utils/height.hpp | 36 ++++++++ common/geography_utils/package.xml | 22 +++++ common/geography_utils/src/height.cpp | 63 +++++++++++++ common/geography_utils/test/test_height.cpp | 92 +++++++++++++++++++ .../gnss_poser/include/gnss_poser/convert.hpp | 47 +++++----- sensing/gnss_poser/package.xml | 1 + system/default_ad_api/package.xml | 1 + system/default_ad_api/src/vehicle.cpp | 10 +- 10 files changed, 284 insertions(+), 28 deletions(-) create mode 100644 common/geography_utils/CMakeLists.txt create mode 100644 common/geography_utils/README.md create mode 100644 common/geography_utils/include/geography_utils/height.hpp create mode 100644 common/geography_utils/package.xml create mode 100644 common/geography_utils/src/height.cpp create mode 100644 common/geography_utils/test/test_height.cpp diff --git a/common/geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt new file mode 100644 index 0000000000000..5cc2290636157 --- /dev/null +++ b/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() diff --git a/common/geography_utils/README.md b/common/geography_utils/README.md new file mode 100644 index 0000000000000..fb4c2dc3a8312 --- /dev/null +++ b/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. diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp new file mode 100644 index 0000000000000..1921e79699970 --- /dev/null +++ b/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 +#include +#include +#include + +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_ diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml new file mode 100644 index 0000000000000..61ac473498632 --- /dev/null +++ b/common/geography_utils/package.xml @@ -0,0 +1,22 @@ + + + + geography_utils + 0.1.0 + The geography_utils package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geographiclib + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp new file mode 100644 index 0000000000000..fe69557f25a76 --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include + +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, 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 diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp new file mode 100644 index 0000000000000..3d8a4c9c203fe --- /dev/null +++ b/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 + +#include + +#include +#include + +// 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(); +} diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 74c9734833232..4dda5d1276c54 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -38,23 +39,7 @@ enum class MGRSPrecision { _1_MIllI_METER = 8, _100MICRO_METER = 9, }; -// EllipsoidHeight:height above ellipsoid -// OrthometricHeight:height above geoid -double EllipsoidHeight2OrthometricHeight( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger) -{ - double OrthometricHeight{0.0}; - try { - GeographicLib::Geoid egm2008("egm2008-1"); - OrthometricHeight = egm2008.ConvertHeight( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, - GeographicLib::Geoid::ELLIPSOIDTOGEOID); - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM( - logger, "Failed to convert Height from Ellipsoid to Orthometric" << err.what()); - } - return OrthometricHeight; -} + GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, int height_system) @@ -65,11 +50,16 @@ GNSSStat NavSatFix2UTM( GeographicLib::UTMUPS::Forward( nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, utm.y); + + std::string target_height_system; if (height_system == 0) { - utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); + target_height_system = "EGM2008"; } else { - utm.z = nav_sat_fix_msg.altitude; + target_height_system = "WGS84"; } + utm.z = geography_utils::convert_height( + nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84", + target_height_system); utm.latitude = nav_sat_fix_msg.latitude; utm.longitude = nav_sat_fix_msg.longitude; utm.altitude = nav_sat_fix_msg.altitude; @@ -78,6 +68,7 @@ GNSSStat NavSatFix2UTM( } return utm; } + GNSSStat NavSatFix2LocalCartesianUTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) @@ -89,11 +80,15 @@ GNSSStat NavSatFix2LocalCartesianUTM( GeographicLib::UTMUPS::Forward( nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, utm_origin.east_north_up, utm_origin.x, utm_origin.y); + std::string target_height_system; if (height_system == 0) { - utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger); + target_height_system = "EGM2008"; } else { - utm_origin.z = nav_sat_fix_origin.altitude; + target_height_system = "WGS84"; } + utm_origin.z = geography_utils::convert_height( + nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, + "WGS84", target_height_system); // individual coordinates of global coordinate system double global_x = 0.0; @@ -107,17 +102,17 @@ GNSSStat NavSatFix2LocalCartesianUTM( // individual coordinates of local coordinate system utm_local.x = global_x - utm_origin.x; utm_local.y = global_y - utm_origin.y; - if (height_system == 0) { - utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z; - } else { - utm_local.z = nav_sat_fix_msg.altitude - utm_origin.z; - } + utm_local.z = geography_utils::convert_height( + nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, + "WGS84", target_height_system) - + utm_origin.z; } catch (const GeographicLib::GeographicErr & err) { RCLCPP_ERROR_STREAM( logger, "Failed to convert from LLH to UTM in local coordinates" << err.what()); } return utm_local; } + GNSSStat UTM2MGRS( const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger) { diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 1488f329edecb..77380bf5492a1 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -15,6 +15,7 @@ autoware_sensing_msgs geographiclib + geography_utils geometry_msgs rclcpp rclcpp_components diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index dfceeabe41122..fd61d3c62e397 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -23,6 +23,7 @@ autoware_planning_msgs component_interface_specs component_interface_utils + geography_utils lanelet2_extension motion_utils nav_msgs diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index fb434adbb0e40..c8f7df315c487 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,6 +14,8 @@ #include "vehicle.hpp" +#include + #include namespace default_ad_api @@ -153,7 +155,9 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; + vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( + projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, + map_projector_info_->vertical_datum, "WGS84"); } else if (map_projector_info_->projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { lanelet::GPSPoint position{ map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; @@ -165,7 +169,9 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; + vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( + projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, + map_projector_info_->vertical_datum, "WGS84"); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = From 5f04a370b2712aa9c42943e4d2866a1577169d6f Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Wed, 30 Aug 2023 20:59:28 +0900 Subject: [PATCH 3/6] fix(lane_change): fix lane_change document (#4817) Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> --- .../docs/behavior_path_planner_lane_change_design.md | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index fddd535931d94..2a04580aef5b5 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/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] | From 058680d2eb17df82d3ca79961c0c5e2b483b842e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 30 Aug 2023 23:12:54 +0900 Subject: [PATCH 4/6] fix(goal_planner): set correct reference path (#4809) Signed-off-by: satoshi-ota --- .../scene_module/goal_planner/goal_planner_module.cpp | 10 ++++++++++ .../utils/goal_planner/default_fixed_goal_planner.cpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f37149f706436..3574abe7b1185 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -520,8 +520,13 @@ void GoalPlannerModule::generateGoalCandidates() BehaviorModuleOutput GoalPlannerModule::plan() { + resetPathCandidate(); + resetPathReference(); + generateGoalCandidates(); + path_reference_ = getPreviousModuleOutput().reference_path; + if (goal_planner_utils::isAllowedGoalModification( planner_data_->route_handler, left_side_parking_)) { return planWithGoalModification(); @@ -840,6 +845,11 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { + resetPathCandidate(); + resetPathReference(); + + path_reference_ = getPreviousModuleOutput().reference_path; + if (goal_planner_utils::isAllowedGoalModification( planner_data_->route_handler, left_side_parking_)) { return planWaitingApprovalWithGoalModification(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 5d77027efa5d0..7efdbdf1552d5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -35,7 +35,7 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(*(output.path), planner_data); output.path = std::make_shared(smoothed_path); - output.reference_path = std::make_shared(smoothed_path); + output.reference_path = getPreviousModuleOutput().reference_path; return output; } From 54b3958489b8f527f49ed4addcc57431be796e2f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 31 Aug 2023 01:04:35 +0900 Subject: [PATCH 5/6] feat(dynamic_avoidance): generate drivable area by ego/object-path-based depending on the case (#4813) * feat(dynamic_avoidance): generate drivable area by ego/object-path-based depending on the case Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * apply clang-format Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 18 +++-- .../dynamic_avoidance_module.cpp | 74 +++++++++++++------ .../dynamic_avoidance/manager.cpp | 5 -- 3 files changed, 65 insertions(+), 32 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 3c302d934735c..fccc27051380e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/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 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 & 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 & prev_object) const; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 0bfda71a09079..da98c0478e580 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -63,7 +63,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po } void appendExtractedPolygonMarker( - MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly) + MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly, const double obj_z) { auto marker = tier4_autoware_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), @@ -78,6 +78,7 @@ void appendExtractedPolygonMarker( geometry_msgs::msg::Point bound_geom_point; bound_geom_point.x = bound_point.x(); bound_geom_point.y = bound_point.y(); + bound_geom_point.z = obj_z; marker.points.push_back(bound_geom_point); } @@ -151,6 +152,25 @@ double calcDiffAngleAgainstPath( return diff_yaw; } +double calcDiffAngleBetweenPaths( + const std::vector & path_points, const PredictedPath & predicted_path) +{ + const size_t nearest_idx = + motion_utils::findNearestSegmentIndex(path_points, predicted_path.path.front().position); + const double ego_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); + + constexpr size_t max_predicted_path_size = 5; + double signed_max_angle{0.0}; + for (size_t i = 0; i < std::min(max_predicted_path_size, predicted_path.path.size()); ++i) { + const double obj_yaw = tf2::getYaw(predicted_path.path.at(i).orientation); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - ego_yaw); + if (std::abs(signed_max_angle) < std::abs(diff_yaw)) { + signed_max_angle = diff_yaw; + } + } + return signed_max_angle; +} + double calcDistanceToPath( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) @@ -281,10 +301,10 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() std::vector obstacles_for_drivable_area; for (const auto & object : target_objects_) { const auto obstacle_poly = [&]() { - if (parameters_->polygon_generation_method == "ego_path_base") { + if (object.polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) { return calcEgoPathBasedDynamicObstaclePolygon(object); } - if (parameters_->polygon_generation_method == "object_path_base") { + if (object.polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) { return calcObjectPathBasedDynamicObstaclePolygon(object); } throw std::logic_error("The polygon_generation_method's string is invalid."); @@ -294,7 +314,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() {object.pose, obstacle_poly.value(), object.is_collision_left}); appendObjectMarker(info_marker_, object.pose); - appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); + appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z); } } @@ -375,6 +395,10 @@ void DynamicAvoidanceModule::updateTargetObjects() predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); + const auto obj_path = *std::max_element( + predicted_object.kinematics.predicted_paths.begin(), + predicted_object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); // 1.a. check label const bool is_label_target_obstacle = @@ -391,7 +415,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); + const double obj_angle = calcDiffAngleBetweenPaths(prev_module_path->points, obj_path); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -448,6 +472,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { + PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::EGO_PATH_BASE}; const auto obj_uuid = object.uuid; const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); const auto obj_path = *std::max_element( @@ -472,8 +497,8 @@ void DynamicAvoidanceModule::updateTargetObjects() getLateralLongitudinalOffset(prev_module_path->points, object.pose, object.shape); // 2.c. check if object will not cut in - const bool will_object_cut_in = - willObjectCutIn(prev_module_path->points, obj_path, object.vel, lat_lon_offset); + const bool will_object_cut_in = willObjectCutIn( + prev_module_path->points, obj_path, object.vel, lat_lon_offset, polygon_generation_method); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -551,7 +576,8 @@ void DynamicAvoidanceModule::updateTargetObjects() const bool should_be_avoided = true; target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + polygon_generation_method); } } @@ -627,16 +653,26 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( bool DynamicAvoidanceModule::willObjectCutIn( const std::vector & 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 { - constexpr double epsilon_path_lat_diff = 0.3; - - // Ignore oncoming object - if (obj_tangent_vel < 0) { + // Check if ego's path and object's path are close. + const bool will_object_cut_in = [&]() { + for (const auto & predicted_path_point : predicted_path.path) { + const double paths_lat_diff = + motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + if (std::abs(paths_lat_diff) < planner_data_->parameters.vehicle_width / 2.0) { + return true; + } + } + return false; + }(); + if (!will_object_cut_in) { + // The object's path will not cut in return false; } - // Ignore object close to the ego + // Ignore object longitudinally close to the ego const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const double lon_offset_ego_to_obj = @@ -647,17 +683,11 @@ bool DynamicAvoidanceModule::willObjectCutIn( lon_offset_ego_to_obj < std::max( parameters_->min_lon_offset_ego_to_cut_in_object, relative_velocity * parameters_->min_time_to_start_cut_in)) { + polygon_generation_method = PolygonGenerationMethod::EGO_PATH_BASE; return false; } - for (const auto & predicted_path_point : predicted_path.path) { - const double paths_lat_diff = - motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); - if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { - return true; - } - } - return false; + return true; } DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index c77d81457c15d..82ebf59ea5bb2 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -81,8 +81,6 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; - p.polygon_generation_method = - node->declare_parameter(ns + "polygon_generation_method"); p.min_obj_path_based_lon_polygon_margin = node->declare_parameter(ns + "object_path_base.min_longitudinal_polygon_margin"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); @@ -181,9 +179,6 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; - - updateParam( - parameters, ns + "polygon_generation_method", p->polygon_generation_method); updateParam( parameters, ns + "object_path_base.min_longitudinal_polygon_margin", p->min_obj_path_based_lon_polygon_margin); From 0b2afec2610c46aa363672f11ad0a88f660e9c1b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 31 Aug 2023 01:04:51 +0900 Subject: [PATCH 6/6] fix(crosswalk): fix plotter data (#4822) Signed-off-by: Takayuki Murooka --- .../scripts/time_to_collision_plotter.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py index 5093683e17c22..8a9de1a563249 100755 --- a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py +++ b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py @@ -109,7 +109,9 @@ def on_collision_info(self, msg): return for i in range(int(len(collision_info_str_vec) / 5)): - collision_info_data_vec.append(CollisionInfo(*collision_info_str_vec[i : i + 5])) + collision_info_data_vec.append( + CollisionInfo(*collision_info_str_vec[i * 5 : i * 5 + 5]) + ) # memorize data in the history dictionary for collision_info_data in collision_info_data_vec: