diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index fc6685f2cafa3..d5417133956f7 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,8 +1,6 @@ *:*/test/* checkersReport -functionConst -functionStatic missingInclude missingIncludeSystem noConstructor @@ -10,7 +8,6 @@ unknownMacro unmatchedSuppression unreadVariable unusedFunction -unusedStructMember useInitializationList useStlAlgorithm variableScope diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index caeebe87193cf..b8104c1bdce99 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,9 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/autoware_geography_utils/** koji.minoda@tier4.jp common/autoware_grid_map_utils/** maxime.clement@tier4.jp +common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai @@ -17,12 +19,10 @@ common/component_interface_tools/** isamu.takagi@tier4.jp common/component_interface_utils/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/geography_utils/** koji.minoda@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp -common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/perception_utils/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp diff --git a/common/geography_utils/CMakeLists.txt b/common/autoware_geography_utils/CMakeLists.txt similarity index 70% rename from common/geography_utils/CMakeLists.txt rename to common/autoware_geography_utils/CMakeLists.txt index b705e819c059b..b4ab5c2f74494 100644 --- a/common/geography_utils/CMakeLists.txt +++ b/common/autoware_geography_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(geography_utils) +project(autoware_geography_utils) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,13 +12,13 @@ find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) find_library(GeographicLib_LIBRARIES NAMES Geographic) -ament_auto_add_library(geography_utils SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/height.cpp src/projection.cpp src/lanelet2_projector.cpp ) -target_link_libraries(geography_utils +target_link_libraries(${PROJECT_NAME} ${GeographicLib_LIBRARIES} ) @@ -27,10 +27,10 @@ if(BUILD_TESTING) file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_geography_utils ${test_files}) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - target_link_libraries(test_geography_utils - geography_utils + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} ) endif() diff --git a/common/geography_utils/README.md b/common/autoware_geography_utils/README.md similarity index 100% rename from common/geography_utils/README.md rename to common/autoware_geography_utils/README.md diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp similarity index 83% rename from common/geography_utils/include/geography_utils/height.hpp rename to common/autoware_geography_utils/include/autoware/geography_utils/height.hpp index 8bcda41f84cda..1f205eb8f8b18 100644 --- a/common/geography_utils/include/geography_utils/height.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GEOGRAPHY_UTILS__HEIGHT_HPP_ -#define GEOGRAPHY_UTILS__HEIGHT_HPP_ +#ifndef AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ #include -namespace geography_utils +namespace autoware::geography_utils { typedef double (*HeightConversionFunction)( @@ -28,6 +28,6 @@ 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 +} // namespace autoware::geography_utils -#endif // GEOGRAPHY_UTILS__HEIGHT_HPP_ +#endif // AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp similarity index 77% rename from common/geography_utils/include/geography_utils/lanelet2_projector.hpp rename to common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp index 739abe7019023..7f6b2d78701d2 100644 --- a/common/geography_utils/include/geography_utils/lanelet2_projector.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ #include @@ -21,12 +21,12 @@ #include -namespace geography_utils +namespace autoware::geography_utils { using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info); -} // namespace geography_utils +} // namespace autoware::geography_utils -#endif // GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#endif // AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp similarity index 82% rename from common/geography_utils/include/geography_utils/projection.hpp rename to common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp index be616d854a466..5ad605f95f65b 100644 --- a/common/geography_utils/include/geography_utils/projection.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GEOGRAPHY_UTILS__PROJECTION_HPP_ -#define GEOGRAPHY_UTILS__PROJECTION_HPP_ +#ifndef AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ #include #include #include -namespace geography_utils +namespace autoware::geography_utils { using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo; using GeoPoint = geographic_msgs::msg::GeoPoint; @@ -28,6 +28,6 @@ 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 +} // namespace autoware::geography_utils -#endif // GEOGRAPHY_UTILS__PROJECTION_HPP_ +#endif // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/common/geography_utils/package.xml b/common/autoware_geography_utils/package.xml similarity index 88% rename from common/geography_utils/package.xml rename to common/autoware_geography_utils/package.xml index bf2356a7fabf5..bd3aa79bde8c2 100644 --- a/common/geography_utils/package.xml +++ b/common/autoware_geography_utils/package.xml @@ -1,9 +1,9 @@ - geography_utils + autoware_geography_utils 0.1.0 - The geography_utils package + The autoware_geography_utils package Koji Minoda Apache License 2.0 diff --git a/common/geography_utils/src/height.cpp b/common/autoware_geography_utils/src/height.cpp similarity index 94% rename from common/geography_utils/src/height.cpp rename to common/autoware_geography_utils/src/height.cpp index fe69557f25a76..745dbf5b22cfc 100644 --- a/common/geography_utils/src/height.cpp +++ b/common/autoware_geography_utils/src/height.cpp @@ -13,14 +13,14 @@ // limitations under the License. #include -#include +#include #include #include #include #include -namespace geography_utils +namespace autoware::geography_utils { double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) @@ -60,4 +60,4 @@ double convert_height( } } -} // namespace geography_utils +} // namespace autoware::geography_utils diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/autoware_geography_utils/src/lanelet2_projector.cpp similarity index 94% rename from common/geography_utils/src/lanelet2_projector.cpp rename to common/autoware_geography_utils/src/lanelet2_projector.cpp index ea32e08cd157a..7de0935a3aa4e 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/autoware_geography_utils/src/lanelet2_projector.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include +#include #include #include -#include #include -namespace geography_utils +namespace autoware::geography_utils { std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) @@ -51,4 +51,4 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf throw std::invalid_argument(error_msg); } -} // namespace geography_utils +} // namespace autoware::geography_utils diff --git a/common/geography_utils/src/projection.cpp b/common/autoware_geography_utils/src/projection.cpp similarity index 95% rename from common/geography_utils/src/projection.cpp rename to common/autoware_geography_utils/src/projection.cpp index d32252c28f5c0..3ab18b1d31698 100644 --- a/common/geography_utils/src/projection.cpp +++ b/common/autoware_geography_utils/src/projection.cpp @@ -13,11 +13,11 @@ // limitations under the License. #include +#include +#include #include -#include -#include -namespace geography_utils +namespace autoware::geography_utils { Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) @@ -92,4 +92,4 @@ GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo return geo_point; } -} // namespace geography_utils +} // namespace autoware::geography_utils diff --git a/common/geography_utils/test/test_geography_utils.cpp b/common/autoware_geography_utils/test/test_geography_utils.cpp similarity index 82% rename from common/geography_utils/test/test_geography_utils.cpp rename to common/autoware_geography_utils/test/test_geography_utils.cpp index 480e17b8f49a4..ee0e7428db2f2 100644 --- a/common/geography_utils/test/test_geography_utils.cpp +++ b/common/autoware_geography_utils/test/test_geography_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "geography_utils/height.hpp" -#include "geography_utils/lanelet2_projector.hpp" -#include "geography_utils/projection.hpp" +#include "autoware/geography_utils/height.hpp" +#include "autoware/geography_utils/lanelet2_projector.hpp" +#include "autoware/geography_utils/projection.hpp" #include diff --git a/common/geography_utils/test/test_height.cpp b/common/autoware_geography_utils/test/test_height.cpp similarity index 80% rename from common/geography_utils/test/test_height.cpp rename to common/autoware_geography_utils/test/test_height.cpp index 15297089ee131..f624f6c3ff9e3 100644 --- a/common/geography_utils/test/test_height.cpp +++ b/common/autoware_geography_utils/test/test_height.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -28,7 +28,7 @@ TEST(GeographyUtils, SameSourceTargetDatum) const std::string datum = "WGS84"; double converted_height = - geography_utils::convert_height(height, latitude, longitude, datum, datum); + autoware::geography_utils::convert_height(height, latitude, longitude, datum, datum); EXPECT_DOUBLE_EQ(height, converted_height); } @@ -44,7 +44,7 @@ TEST(GeographyUtils, ValidSourceTargetDatum) const double target_height = -30.18; double converted_height = - geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); EXPECT_NEAR(target_height, converted_height, 0.1); } @@ -57,7 +57,7 @@ TEST(GeographyUtils, InvalidSourceTargetDatum) const double longitude = 139.0; EXPECT_THROW( - geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), + autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), std::invalid_argument); } @@ -69,7 +69,7 @@ TEST(GeographyUtils, InvalidSourceDatum) const double longitude = 139.0; EXPECT_THROW( - geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), + autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), std::invalid_argument); } @@ -81,6 +81,6 @@ TEST(GeographyUtils, InvalidTargetDatum) const double longitude = 139.0; EXPECT_THROW( - geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), + autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), std::invalid_argument); } diff --git a/common/geography_utils/test/test_projection.cpp b/common/autoware_geography_utils/test/test_projection.cpp similarity index 89% rename from common/geography_utils/test/test_projection.cpp rename to common/autoware_geography_utils/test/test_projection.cpp index 74ffb616cde6f..3355dbf8a50ea 100644 --- a/common/geography_utils/test/test_projection.cpp +++ b/common/autoware_geography_utils/test/test_projection.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -41,7 +41,7 @@ TEST(GeographyUtilsProjection, ProjectForwardToMGRS) // conversion const geometry_msgs::msg::Point converted_point = - geography_utils::project_forward(geo_point, projector_info); + autoware::geography_utils::project_forward(geo_point, projector_info); EXPECT_NEAR(converted_point.x, local_point.x, 1.0); EXPECT_NEAR(converted_point.y, local_point.y, 1.0); @@ -70,7 +70,7 @@ TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) // conversion const geographic_msgs::msg::GeoPoint converted_point = - geography_utils::project_reverse(local_point, projector_info); + autoware::geography_utils::project_reverse(local_point, projector_info); EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); @@ -93,9 +93,9 @@ TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) // conversion const geometry_msgs::msg::Point converted_local_point = - geography_utils::project_forward(geo_point, projector_info); + autoware::geography_utils::project_forward(geo_point, projector_info); const geographic_msgs::msg::GeoPoint converted_geo_point = - geography_utils::project_reverse(converted_local_point, projector_info); + autoware::geography_utils::project_reverse(converted_local_point, projector_info); EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); @@ -126,7 +126,7 @@ TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) // conversion const geometry_msgs::msg::Point converted_point = - geography_utils::project_forward(geo_point, projector_info); + autoware::geography_utils::project_forward(geo_point, projector_info); EXPECT_NEAR(converted_point.x, local_point.x, 1.0); EXPECT_NEAR(converted_point.y, local_point.y, 1.0); @@ -151,9 +151,9 @@ TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) // conversion const geometry_msgs::msg::Point converted_local_point = - geography_utils::project_forward(geo_point, projector_info); + autoware::geography_utils::project_forward(geo_point, projector_info); const geographic_msgs::msg::GeoPoint converted_geo_point = - geography_utils::project_reverse(converted_local_point, projector_info); + autoware::geography_utils::project_reverse(converted_local_point, projector_info); EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); diff --git a/common/kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt similarity index 54% rename from common/kalman_filter/CMakeLists.txt rename to common/autoware_kalman_filter/CMakeLists.txt index 6cbdf67a0affa..076d2d3cad4e8 100644 --- a/common/kalman_filter/CMakeLists.txt +++ b/common/autoware_kalman_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(kalman_filter) +project(autoware_kalman_filter) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,18 +12,18 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_library(kalman_filter SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/kalman_filter.cpp src/time_delay_kalman_filter.cpp - include/kalman_filter/kalman_filter.hpp - include/kalman_filter/time_delay_kalman_filter.hpp + include/autoware/kalman_filter/kalman_filter.hpp + include/autoware/kalman_filter/time_delay_kalman_filter.hpp ) if(BUILD_TESTING) file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_kalman_filter ${test_files}) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - target_link_libraries(test_kalman_filter kalman_filter) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() ament_auto_package() diff --git a/common/kalman_filter/README.md b/common/autoware_kalman_filter/README.md similarity index 100% rename from common/kalman_filter/README.md rename to common/autoware_kalman_filter/README.md diff --git a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp similarity index 96% rename from common/kalman_filter/include/kalman_filter/kalman_filter.hpp rename to common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp index b500cffb92279..74db04f6e838b 100644 --- a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp +++ b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KALMAN_FILTER__KALMAN_FILTER_HPP_ -#define KALMAN_FILTER__KALMAN_FILTER_HPP_ +#ifndef AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ +#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ #include #include +namespace autoware::kalman_filter +{ + /** * @file kalman_filter.h * @brief kalman filter class @@ -207,4 +210,5 @@ class KalmanFilter Eigen::MatrixXd R_; //!< @brief covariance matrix for measurement model y[k] = C * x[k] Eigen::MatrixXd P_; //!< @brief covariance of estimated state }; -#endif // KALMAN_FILTER__KALMAN_FILTER_HPP_ +} // namespace autoware::kalman_filter +#endif // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ diff --git a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp similarity index 89% rename from common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp rename to common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp index cdc03f3558854..80375b7579e62 100644 --- a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp +++ b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp @@ -12,16 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ -#define KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ +#ifndef AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ +#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ -#include "kalman_filter/kalman_filter.hpp" +#include "autoware/kalman_filter/kalman_filter.hpp" #include #include #include +namespace autoware::kalman_filter +{ /** * @file time_delay_kalman_filter.h * @brief kalman filter with delayed measurement class @@ -83,4 +85,5 @@ class TimeDelayKalmanFilter : public KalmanFilter int dim_x_; //!< @brief dimension of latest state int dim_x_ex_; //!< @brief dimension of extended state with dime delay }; -#endif // KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ +} // namespace autoware::kalman_filter +#endif // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ diff --git a/common/kalman_filter/package.xml b/common/autoware_kalman_filter/package.xml similarity index 96% rename from common/kalman_filter/package.xml rename to common/autoware_kalman_filter/package.xml index 200440b5774c7..7d36bfc47aecf 100644 --- a/common/kalman_filter/package.xml +++ b/common/autoware_kalman_filter/package.xml @@ -1,7 +1,7 @@ - kalman_filter + autoware_kalman_filter 0.1.0 The kalman filter package Yukihiro Saito diff --git a/common/kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp similarity index 96% rename from common/kalman_filter/src/kalman_filter.cpp rename to common/autoware_kalman_filter/src/kalman_filter.cpp index 450d40936db2e..bbd963675f9e2 100644 --- a/common/kalman_filter/src/kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/kalman_filter.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kalman_filter/kalman_filter.hpp" +#include "autoware/kalman_filter/kalman_filter.hpp" +namespace autoware::kalman_filter +{ KalmanFilter::KalmanFilter() { } @@ -156,3 +158,4 @@ bool KalmanFilter::update(const Eigen::MatrixXd & y) { return update(y, C_, R_); } +} // namespace autoware::kalman_filter diff --git a/common/kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp similarity index 95% rename from common/kalman_filter/src/time_delay_kalman_filter.cpp rename to common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp index c4143429e6c05..31c1c768d7173 100644 --- a/common/kalman_filter/src/time_delay_kalman_filter.cpp +++ b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kalman_filter/time_delay_kalman_filter.hpp" +#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" +namespace autoware::kalman_filter +{ TimeDelayKalmanFilter::TimeDelayKalmanFilter() { } @@ -102,3 +104,4 @@ bool TimeDelayKalmanFilter::updateWithDelay( return true; } +} // namespace autoware::kalman_filter diff --git a/common/kalman_filter/test/test_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_kalman_filter.cpp similarity index 96% rename from common/kalman_filter/test/test_kalman_filter.cpp rename to common/autoware_kalman_filter/test/test_kalman_filter.cpp index 4f4e9ce44669b..34e23ef9d06e2 100644 --- a/common/kalman_filter/test/test_kalman_filter.cpp +++ b/common/autoware_kalman_filter/test/test_kalman_filter.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kalman_filter/kalman_filter.hpp" +#include "autoware/kalman_filter/kalman_filter.hpp" #include +using autoware::kalman_filter::KalmanFilter; + TEST(kalman_filter, kf) { KalmanFilter kf_; diff --git a/common/kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp similarity index 97% rename from common/kalman_filter/test/test_time_delay_kalman_filter.cpp rename to common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp index 32fefd8ceff70..50c22fae123bc 100644 --- a/common/kalman_filter/test/test_time_delay_kalman_filter.cpp +++ b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kalman_filter/time_delay_kalman_filter.hpp" +#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" #include +using autoware::kalman_filter::TimeDelayKalmanFilter; + TEST(time_delay_kalman_filter, td_kf) { TimeDelayKalmanFilter td_kf_; diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index 2db4e6a9e9162..fd18d5c620832 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -402,6 +403,7 @@ void createPublisherWithQoS( rclcpp::Node::SharedPtr test_node, std::string topic_name, std::shared_ptr> & publisher) { + // override QoS settings for specific message types if constexpr ( std::is_same_v || std::is_same_v || std::is_same_v) { @@ -443,18 +445,23 @@ void setPublisher( * @param topic_name The name of the topic to subscribe to. * @param callback The callback function to call when a message is received. * @param subscriber A reference to the subscription to be created. + * @param qos The QoS settings for the subscription (optional). */ template void createSubscription( - rclcpp::Node::SharedPtr test_node, std::string topic_name, + rclcpp::Node::SharedPtr test_node, const std::string & topic_name, std::function callback, - std::shared_ptr> & subscriber) + std::shared_ptr> & subscriber, + std::optional qos = std::nullopt) { - if constexpr (std::is_same_v) { - subscriber = test_node->create_subscription(topic_name, rclcpp::QoS{1}, callback); - } else { - subscriber = test_node->create_subscription(topic_name, 10, callback); + if (!qos.has_value()) { + if constexpr (std::is_same_v) { + qos = rclcpp::QoS{1}; + } else { + qos = rclcpp::QoS{10}; + } } + subscriber = test_node->create_subscription(topic_name, *qos, callback); } /** @@ -467,14 +474,17 @@ void createSubscription( * @param topic_name The name of the topic to subscribe to. * @param subscriber A reference to the subscription to be set. * @param count A reference to a counter that increments on message receipt. + * @param qos The QoS settings for the subscription (optional). */ template void setSubscriber( rclcpp::Node::SharedPtr test_node, std::string topic_name, - std::shared_ptr> & subscriber, size_t & count) + std::shared_ptr> & subscriber, size_t & count, + std::optional qos = std::nullopt) { createSubscription( - test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber); + test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber, + qos); } /** @@ -534,28 +544,40 @@ class AutowareTestManager template void test_pub_msg( rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg) + { + rclcpp::QoS qos(rclcpp::KeepLast(10)); + test_pub_msg(target_node, topic_name, msg, qos); + } + + template + void test_pub_msg( + rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg, + rclcpp::QoS qos) { if (publishers_.find(topic_name) == publishers_.end()) { - auto publisher = test_node_->create_publisher(topic_name, 10); + auto publisher = test_node_->create_publisher(topic_name, qos); publishers_[topic_name] = std::static_pointer_cast(publisher); } auto publisher = std::dynamic_pointer_cast>(publishers_[topic_name]); - autoware::test_utils::publishToTargetNode(test_node_, target_node, topic_name, publisher, msg); + publisher->publish(msg); + const int repeat_count = 3; + autoware::test_utils::spinSomeNodes(test_node_, target_node, repeat_count); RCLCPP_INFO(test_node_->get_logger(), "Published message on topic '%s'", topic_name.c_str()); } template void set_subscriber( const std::string & topic_name, - std::function callback) + std::function callback, + std::optional qos = std::nullopt) { if (subscribers_.find(topic_name) == subscribers_.end()) { std::shared_ptr> subscriber; autoware::test_utils::createSubscription( - test_node_, topic_name, callback, subscriber); + test_node_, topic_name, callback, subscriber, qos); subscribers_[topic_name] = std::static_pointer_cast(subscriber); } else { RCLCPP_WARN(test_node_->get_logger(), "Subscriber %s already set.", topic_name.c_str()); diff --git a/common/autoware_universe_utils/examples/example_lru_cache.cpp b/common/autoware_universe_utils/examples/example_lru_cache.cpp new file mode 100644 index 0000000000000..95decf8eb336d --- /dev/null +++ b/common/autoware_universe_utils/examples/example_lru_cache.cpp @@ -0,0 +1,73 @@ +// Copyright 2024 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 "autoware/universe_utils/system/lru_cache.hpp" + +#include +#include +#include + +using autoware::universe_utils::LRUCache; + +// Fibonacci calculation with LRU cache +int64_t fibonacci_with_cache(int n, LRUCache * cache) +{ + if (n <= 1) return n; + + if (cache->contains(n)) { + return *cache->get(n); + } + int64_t result = fibonacci_with_cache(n - 1, cache) + fibonacci_with_cache(n - 2, cache); + cache->put(n, result); + return result; +} + +// Fibonacci calculation without cache +int64_t fibonacci_no_cache(int n) +{ + if (n <= 1) return n; + return fibonacci_no_cache(n - 1) + fibonacci_no_cache(n - 2); +} + +// Helper function to measure execution time +template +int64_t measure_time(Func func, Args &&... args) +{ + auto start = std::chrono::high_resolution_clock::now(); + func(std::forward(args)...); + auto end = std::chrono::high_resolution_clock::now(); + return std::chrono::duration_cast(end - start).count(); +} + +int main() +{ + const int max_n = 40; // Increased for more noticeable time difference + LRUCache cache(max_n); // Cache size set to MAX_N + + std::cout << "Comparing Fibonacci calculation time with and without LRU cache:\n\n"; + std::cout << "n\tWith Cache (μs)\tWithout Cache (μs)\n"; + std::cout << std::string(43, '-') << "\n"; + + for (int i = 30; i <= max_n; ++i) { // Starting from 30 for more significant differences + int64_t time_with_cache = measure_time([i, &cache]() { fibonacci_with_cache(i, &cache); }); + int64_t time_without_cache = measure_time([i]() { fibonacci_no_cache(i); }); + + std::cout << i << "\t" << time_with_cache << "\t\t" << time_without_cache << "\n"; + + // Clear the cache after each iteration to ensure fair comparison + cache.clear(); + } + + return 0; +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp new file mode 100644 index 0000000000000..ecd8fc7e6c01a --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp @@ -0,0 +1,142 @@ +// Copyright 2024 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 AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_ + +#include +#include +#include +#include +#include + +namespace autoware::universe_utils +{ + +/** + * @brief A template class for LRU (Least Recently Used) Cache. + * + * This class implements a simple LRU cache using a combination of a list and a hash map. + * + * @tparam Key The type of keys. + * @tparam Value The type of values. + * @tparam Map The type of underlying map, defaulted to std::unordered_map. + */ +template class Map = std::unordered_map> +class LRUCache +{ +private: + size_t capacity_; ///< The maximum capacity of the cache. + std::list> cache_list_; ///< List to maintain the order of elements. + Map>::iterator> + cache_map_; ///< Map for fast access to elements. + +public: + /** + * @brief Construct a new LRUCache object. + * + * @param size The capacity of the cache. + */ + explicit LRUCache(size_t size) : capacity_(size) {} + + /** + * @brief Get the capacity of the cache. + * + * @return The capacity of the cache. + */ + [[nodiscard]] size_t capacity() const { return capacity_; } + + /** + * @brief Insert a key-value pair into the cache. + * + * If the key already exists, its value is updated and it is moved to the front. + * If the cache exceeds its capacity, the least recently used element is removed. + * + * @param key The key to insert. + * @param value The value to insert. + */ + void put(const Key & key, const Value & value) + { + auto it = cache_map_.find(key); + if (it != cache_map_.end()) { + cache_list_.erase(it->second); + } + cache_list_.push_front({key, value}); + cache_map_[key] = cache_list_.begin(); + + if (cache_map_.size() > capacity_) { + auto last = cache_list_.back(); + cache_map_.erase(last.first); + cache_list_.pop_back(); + } + } + + /** + * @brief Retrieve a value from the cache. + * + * If the key does not exist in the cache, std::nullopt is returned. + * If the key exists, the value is returned and the element is moved to the front. + * + * @param key The key to retrieve. + * @return The value associated with the key, or std::nullopt if the key does not exist. + */ + std::optional get(const Key & key) + { + auto it = cache_map_.find(key); + if (it == cache_map_.end()) { + return std::nullopt; + } + cache_list_.splice(cache_list_.begin(), cache_list_, it->second); + return it->second->second; + } + + /** + * @brief Clear the cache. + * + * This removes all elements from the cache. + */ + void clear() + { + cache_list_.clear(); + cache_map_.clear(); + } + + /** + * @brief Get the current size of the cache. + * + * @return The number of elements in the cache. + */ + [[nodiscard]] size_t size() const { return cache_map_.size(); } + + /** + * @brief Check if the cache is empty. + * + * @return True if the cache is empty, false otherwise. + */ + [[nodiscard]] bool empty() const { return cache_map_.empty(); } + + /** + * @brief Check if a key exists in the cache. + * + * @param key The key to check. + * @return True if the key exists, false otherwise. + */ + [[nodiscard]] bool contains(const Key & key) const + { + return cache_map_.find(key) != cache_map_.end(); + } +}; + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_ diff --git a/common/autoware_universe_utils/test/src/system/test_lru_cache.cpp b/common/autoware_universe_utils/test/src/system/test_lru_cache.cpp new file mode 100644 index 0000000000000..f30f732431fa6 --- /dev/null +++ b/common/autoware_universe_utils/test/src/system/test_lru_cache.cpp @@ -0,0 +1,78 @@ +// Copyright 2024 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 "autoware/universe_utils/system/lru_cache.hpp" + +#include + +#include +#include +#include +#include + +using autoware::universe_utils::LRUCache; + +// Fibonacci calculation with LRU cache +int64_t fibonacci_with_cache(int n, LRUCache * cache) +{ + if (n <= 1) return n; + + if (cache->contains(n)) { + return *cache->get(n); + } + int64_t result = fibonacci_with_cache(n - 1, cache) + fibonacci_with_cache(n - 2, cache); + cache->put(n, result); + return result; +} + +// Fibonacci calculation without cache +int64_t fibonacci_no_cache(int n) +{ + if (n <= 1) return n; + return fibonacci_no_cache(n - 1) + fibonacci_no_cache(n - 2); +} + +// Helper function to measure execution time +template +std::pair()(std::declval()...))> measure_time( + Func func, Args &&... args) +{ + auto start = std::chrono::high_resolution_clock::now(); + auto result = func(std::forward(args)...); + auto end = std::chrono::high_resolution_clock::now(); + return {std::chrono::duration_cast(end - start).count(), result}; +} + +// Test case to verify Fibonacci calculation results with and without cache +TEST(FibonacciLRUCacheTest, CompareResultsAndPerformance) +{ + const int max_n = 40; // Test range + LRUCache cache(max_n); // Cache with capacity set to max_n + + for (int i = 0; i <= max_n; ++i) { + // Measure time for performance comparison + auto [time_with_cache, result_with_cache] = + measure_time([i, &cache]() { return fibonacci_with_cache(i, &cache); }); + auto [time_without_cache, result_without_cache] = + measure_time([i]() { return fibonacci_no_cache(i); }); + + EXPECT_EQ(result_with_cache, result_without_cache) << "Mismatch at n = " << i; + + // Print the calculation time + std::cout << "n = " << i << ": With Cache = " << time_with_cache + << " μs, Without Cache = " << time_without_cache << " μs\n"; + + // // Clear the cache after each iteration to ensure fair comparison + cache.clear(); + } +} diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt index d4bbe928c0be8..18ef18303fb1b 100644 --- a/common/perception_utils/CMakeLists.txt +++ b/common/perception_utils/CMakeLists.txt @@ -4,4 +4,13 @@ project(perception_utils) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(${PROJECT_NAME} SHARED + src/run_length_encoder.cpp +) + +find_package(OpenCV REQUIRED) +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBS} +) + ament_auto_package() diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/utils.hpp b/common/perception_utils/include/perception_utils/run_length_encoder.hpp similarity index 79% rename from perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/utils.hpp rename to common/perception_utils/include/perception_utils/run_length_encoder.hpp index 8b86a798e798b..18b5f21854e35 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/utils.hpp +++ b/common/perception_utils/include/perception_utils/run_length_encoder.hpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TENSORRT_YOLOX__UTILS_HPP_ -#define AUTOWARE__TENSORRT_YOLOX__UTILS_HPP_ +#ifndef PERCEPTION_UTILS__RUN_LENGTH_ENCODER_HPP_ + +#define PERCEPTION_UTILS__RUN_LENGTH_ENCODER_HPP_ #include #include #include -namespace autoware::tensorrt_yolox +namespace perception_utils { std::vector> runLengthEncoder(const cv::Mat & mask); cv::Mat runLengthDecoder(const std::vector & rle_data, const int rows, const int cols); -} // namespace autoware::tensorrt_yolox +} // namespace perception_utils -#endif // AUTOWARE__TENSORRT_YOLOX__UTILS_HPP_ +#endif // PERCEPTION_UTILS__RUN_LENGTH_ENCODER_HPP_ diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index a6bfbc42090fe..b38226991eb66 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_cmake + libopencv-dev rclcpp diff --git a/perception/autoware_tensorrt_yolox/src/utils.cpp b/common/perception_utils/src/run_length_encoder.cpp similarity index 94% rename from perception/autoware_tensorrt_yolox/src/utils.cpp rename to common/perception_utils/src/run_length_encoder.cpp index 8d2b4a4a0527f..fb7f5ba33b846 100644 --- a/perception/autoware_tensorrt_yolox/src/utils.cpp +++ b/common/perception_utils/src/run_length_encoder.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/tensorrt_yolox/utils.hpp" +#include "perception_utils/run_length_encoder.hpp" -namespace autoware::tensorrt_yolox +namespace perception_utils { std::vector> runLengthEncoder(const cv::Mat & image) @@ -62,4 +62,4 @@ cv::Mat runLengthDecoder(const std::vector & rle_data, const int rows, return mask; } -} // namespace autoware::tensorrt_yolox +} // namespace perception_utils diff --git a/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp b/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp index 70f3e3736a1a8..f6eaf3ad883f1 100644 --- a/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp +++ b/common/tier4_planning_rviz_plugin/src/tools/jsk_overlay_utils.cpp @@ -116,7 +116,7 @@ OverlayObject::OverlayObject( OverlayObject::~OverlayObject() { - hide(); + OverlayObject::hide(); panel_material_->unload(); Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); diff --git a/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp index 36c5d1ce82693..13eef26598020 100644 --- a/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp +++ b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp @@ -113,7 +113,7 @@ OverlayObject::OverlayObject(const std::string & name) : name_(name) OverlayObject::~OverlayObject() { - hide(); + OverlayObject::hide(); panel_material_->unload(); Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp index 36c5d1ce82693..13eef26598020 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/jsk_overlay_utils.cpp @@ -113,7 +113,7 @@ OverlayObject::OverlayObject(const std::string & name) : name_(name) OverlayObject::~OverlayObject() { - hide(); + OverlayObject::hide(); panel_material_->unload(); Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); diff --git a/control/autoware_external_cmd_selector/README.md b/control/autoware_external_cmd_selector/README.md index 0495004b68b84..0c76afff35e53 100644 --- a/control/autoware_external_cmd_selector/README.md +++ b/control/autoware_external_cmd_selector/README.md @@ -32,3 +32,7 @@ The current mode is set via service, `remote` is remotely operated, `local` is t | `/external/selected/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | | `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | | `/external/selected/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | + +## Parameters + +{{json_to_markdown("control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json")}} diff --git a/control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json b/control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json new file mode 100644 index 0000000000000..b577277695767 --- /dev/null +++ b/control/autoware_external_cmd_selector/schema/external_cmd_selector.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for external_cmd_selector", + "type": "object", + "definitions": { + "external_cmd_selector_node": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10.0, + "description": "The rate in Hz at which the external command selector node updates." + }, + "initial_selector_mode": { + "type": "string", + "enum": ["local", "remote"], + "default": "local", + "description": "The initial mode for command selection, either 'local' or 'remote'." + } + }, + "required": ["update_rate", "initial_selector_mode"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/external_cmd_selector_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 7e39cb6cba8c4..4bcc3f1b7baee 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -60,18 +60,18 @@ typedef boost::geometry::index::rtree #include +#include #include #include #include +#include #include #include #include @@ -78,6 +80,12 @@ class LaneDepartureCheckerNode : public rclcpp::Node this, "~/input/reference_trajectory"}; autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ this, "~/input/predicted_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber< + autoware_adapi_v1_msgs::msg::OperationModeState> + sub_operation_mode_{this, "/api/operation_mode/state"}; + autoware::universe_utils::InterProcessPollingSubscriber< + autoware_vehicle_msgs::msg::ControlModeReport> + sub_control_mode_{this, "/vehicle/status/control_mode"}; // Data Buffer nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; @@ -91,6 +99,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node lanelet::ConstLanelets route_lanelets_; Trajectory::ConstSharedPtr reference_trajectory_; Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; // Callback void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); @@ -98,6 +108,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node void onRoute(const LaneletRoute::ConstSharedPtr msg); void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); + void onOperationMode(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); // Publisher autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml index e0e877e618c65..ea971a35246ad 100644 --- a/control/autoware_lane_departure_checker/package.xml +++ b/control/autoware_lane_departure_checker/package.xml @@ -13,12 +13,14 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils + autoware_vehicle_msgs boost diagnostic_updater eigen diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index c079f014cfdee..8d954517f552d 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -216,6 +216,16 @@ bool LaneDepartureCheckerNode::isDataReady() return false; } + if (!operation_mode_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for operation_mode msg..."); + return false; + } + + if (!control_mode_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for control_mode msg..."); + return false; + } + return true; } @@ -260,6 +270,8 @@ void LaneDepartureCheckerNode::onTimer() route_ = sub_route_.takeData(); reference_trajectory_ = sub_reference_trajectory_.takeData(); predicted_trajectory_ = sub_predicted_trajectory_.takeData(); + operation_mode_ = sub_operation_mode_.takeData(); + control_mode_ = sub_control_mode_.takeData(); const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData(); if (lanelet_map_bin_msg) { @@ -426,6 +438,9 @@ void LaneDepartureCheckerNode::checkTrajectoryDeviation( diagnostic_updater::DiagnosticStatusWrapper & stat) { using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; + using ControlModeStatus = autoware_vehicle_msgs::msg::ControlModeReport; + using OperationModeStatus = autoware_adapi_v1_msgs::msg::OperationModeState; + int8_t level = DiagStatus::OK; if (std::abs(output_.trajectory_deviation.lateral) >= param_.max_lateral_deviation) { @@ -441,8 +456,12 @@ void LaneDepartureCheckerNode::checkTrajectoryDeviation( } std::string msg = "OK"; - if (level == DiagStatus::ERROR) { + if ( + level == DiagStatus::ERROR && operation_mode_->mode == OperationModeStatus::AUTONOMOUS && + control_mode_->mode == ControlModeStatus::AUTONOMOUS) { msg = "trajectory deviation is too large"; + } else { + level = DiagStatus::OK; } stat.addf("max lateral deviation", "%.3f", param_.max_lateral_deviation); diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index a02ab010e8cea..893bf20dd06c5 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -361,12 +361,14 @@ template T VehicleCmdGate::getContinuousTopic( const std::shared_ptr & prev_topic, const T & current_topic, const std::string & topic_name) { - if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() > 0.0) { + if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() >= 0.0) { return current_topic; } else { - RCLCPP_INFO( - get_logger(), - "The operation mode is changed, but the %s is not received yet:", topic_name.c_str()); + if (topic_name != "") { + RCLCPP_INFO( + get_logger(), + "The operation mode is changed, but the %s is not received yet:", topic_name.c_str()); + } return *prev_topic; } } @@ -536,7 +538,6 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) // Set default commands { filtered_commands.control = commands.control; - filtered_commands.gear = commands.gear; // tmp } if (moderate_stop_interface_->is_stop_requested()) { // if stop requested, stop the vehicle @@ -549,7 +550,6 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "Emergency!"); filtered_commands.control = emergency_commands_.control; - filtered_commands.gear = emergency_commands_.gear; // tmp } // Check engage @@ -901,7 +901,6 @@ MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_a } if (filter_activated.is_activated_on_steering_rate) { reason += first_msg ? " steer_rate" : ", steer_rate"; - first_msg = false; } msg.markers.emplace_back(createStringMarker( diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 569136847f1a6..6863015443363 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -240,7 +240,8 @@ class VehicleCmdGate : public rclcpp::Node template T getContinuousTopic( - const std::shared_ptr & prev_topic, const T & current_topic, const std::string & topic_name); + const std::shared_ptr & prev_topic, const T & current_topic, + const std::string & topic_name = ""); // Algorithm Control prev_control_cmd_; diff --git a/evaluator/kinematic_evaluator/CMakeLists.txt b/evaluator/kinematic_evaluator/CMakeLists.txt index 6c951d610bd8b..6c1b6abc3dace 100644 --- a/evaluator/kinematic_evaluator/CMakeLists.txt +++ b/evaluator/kinematic_evaluator/CMakeLists.txt @@ -38,8 +38,7 @@ if(BUILD_TESTING) ) endif() -ament_auto_package( - INSTALL_TO_SHARE +ament_auto_package(INSTALL_TO_SHARE param launch ) diff --git a/evaluator/kinematic_evaluator/README.md b/evaluator/kinematic_evaluator/README.md index 6e7caddd1f5bf..7cc826195c1b4 100644 --- a/evaluator/kinematic_evaluator/README.md +++ b/evaluator/kinematic_evaluator/README.md @@ -1,3 +1,7 @@ # Kinematic Evaluator TBD + +## Parameters + +{{json_to_markdown("evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json")}} diff --git a/evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json b/evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json new file mode 100644 index 0000000000000..86bb8c74f27f6 --- /dev/null +++ b/evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json @@ -0,0 +1,35 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for kinematic_evaluator_node", + "type": "object", + "definitions": { + "kinematic_evaluator_node": { + "type": "object", + "properties": { + "output_file": { + "type": "string", + "default": "kinematic_metrics.results", + "description": "Name of the file to which kinematic metrics are written. If empty, metrics are not written to a file." + }, + "selected_metrics": { + "type": "string", + "default": "velocity_stats", + "description": "The specific metrics selected for evaluation." + } + }, + "required": ["output_file", "selected_metrics"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/kinematic_evaluator_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 8d24e6ef6b294..5f6632e76cdfa 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -9,7 +9,10 @@ - + @@ -128,7 +131,11 @@ - + + + + + diff --git a/localization/autoware_geo_pose_projector/package.xml b/localization/autoware_geo_pose_projector/package.xml index a56a53d609370..2d7c09f9e2ce2 100644 --- a/localization/autoware_geo_pose_projector/package.xml +++ b/localization/autoware_geo_pose_projector/package.xml @@ -17,10 +17,10 @@ ament_cmake_auto autoware_cmake + autoware_geography_utils component_interface_specs component_interface_utils geographic_msgs - geography_utils geometry_msgs rclcpp rclcpp_components diff --git a/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp b/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp index e7fda37e07acd..f44780c140127 100644 --- a/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp +++ b/localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp @@ -14,8 +14,8 @@ #include "geo_pose_projector.hpp" -#include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -66,8 +66,8 @@ void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr m gps_point.longitude = msg->pose.pose.position.longitude; gps_point.altitude = msg->pose.pose.position.altitude; geometry_msgs::msg::Point position = - geography_utils::project_forward(gps_point, projector_info_.value()); - position.z = geography_utils::convert_height( + autoware::geography_utils::project_forward(gps_point, projector_info_.value()); + position.z = autoware::geography_utils::convert_height( position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, projector_info_.value().vertical_datum); diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 5eda1156327b5..a127d2ae81433 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -19,8 +19,8 @@ #include "ekf_localizer/state_index.hpp" #include "ekf_localizer/warning.hpp" -#include -#include +#include +#include #include #include @@ -34,6 +34,8 @@ #include #include +using autoware::kalman_filter::TimeDelayKalmanFilter; + struct EKFDiagnosticInfo { size_t no_update_count{0}; diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index b53538de11f15..34881ddd3c68c 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -22,11 +22,11 @@ eigen + autoware_kalman_filter autoware_universe_utils diagnostic_msgs fmt geometry_msgs - kalman_filter localization_util nav_msgs rclcpp diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 92df806605357..aa412fab317a2 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -66,6 +66,7 @@ #include #include #include +#include #include class NDTScanMatcher : public rclcpp::Node @@ -105,7 +106,7 @@ class NDTScanMatcher : public rclcpp::Node const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); - geometry_msgs::msg::PoseWithCovarianceStamped align_pose( + std::tuple align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); void transform_sensor_measurement( diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a90ec30aa91ff..3e8b9b99737c0 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -994,12 +994,22 @@ void NDTScanMatcher::service_ndt_align_main( return; } - res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame); + // estimate initial pose + const auto [pose_with_covariance, score] = align_pose(initial_pose_msg_in_map_frame); + + // check reliability of initial pose result + res->reliable = + (param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood < score); + if (!res->reliable) { + RCLCPP_WARN_STREAM( + this->get_logger(), "Initial Pose Estimation is Unstable. Score is " << score); + } res->success = true; + res->pose_with_covariance = pose_with_covariance; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; } -geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( +std::tuple NDTScanMatcher::align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); @@ -1089,13 +1099,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( std::begin(particle_array), std::end(particle_array), [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); - if ( - best_particle_ptr->score < - param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood) - RCLCPP_WARN_STREAM( - this->get_logger(), - "Initial Pose Estimation is Unstable. Score is " << best_particle_ptr->score); - geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; @@ -1104,7 +1107,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score); - return result_pose_with_cov_msg; + return std::make_tuple(result_pose_with_cov_msg, best_particle_ptr->score); } #include diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 36cc92e1d235b..9083a6570d33b 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -41,6 +41,14 @@ This node depends on the map height fitter library. | `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state | | `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | +## Diagnostics + +### pose_initializer_status + +If the score of initial pose estimation result is lower than score threshold, ERROR message is output to the `/diagnostics` topic. + +drawing + ## Connection with Default AD API This `pose_initializer` is used via default AD API. For detailed description of the API description, please refer to [the description of `default_ad_api`](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/localization.md). diff --git a/localization/pose_initializer/media/diagnostic_pose_reliability.png b/localization/pose_initializer/media/diagnostic_pose_reliability.png new file mode 100644 index 0000000000000..7e2d8dc5e632a Binary files /dev/null and b/localization/pose_initializer/media/diagnostic_pose_reliability.png differ diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 5f3ea72f7d400..ca4aa032fc7f9 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -24,6 +24,7 @@ component_interface_specs component_interface_utils geometry_msgs + localization_util map_height_fitter rclcpp rclcpp_components diff --git a/localization/pose_initializer/src/pose_initializer/localization_module.cpp b/localization/pose_initializer/src/pose_initializer/localization_module.cpp index 433833cd15784..6963e5adbba3e 100644 --- a/localization/pose_initializer/src/pose_initializer/localization_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/localization_module.cpp @@ -30,7 +30,8 @@ LocalizationModule::LocalizationModule(rclcpp::Node * node, const std::string & cli_align_ = node->create_client(service_name); } -PoseWithCovarianceStamped LocalizationModule::align_pose(const PoseWithCovarianceStamped & pose) +std::tuple LocalizationModule::align_pose( + const PoseWithCovarianceStamped & pose) { const auto req = std::make_shared(); req->pose_with_covariance = pose; @@ -47,5 +48,5 @@ PoseWithCovarianceStamped LocalizationModule::align_pose(const PoseWithCovarianc RCLCPP_INFO(logger_, "align server succeeded."); // Overwrite the covariance. - return res->pose_with_covariance; + return std::make_tuple(res->pose_with_covariance, res->reliable); } diff --git a/localization/pose_initializer/src/pose_initializer/localization_module.hpp b/localization/pose_initializer/src/pose_initializer/localization_module.hpp index f17f68670306c..dc8bd49a26a79 100644 --- a/localization/pose_initializer/src/pose_initializer/localization_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/localization_module.hpp @@ -21,6 +21,7 @@ #include #include +#include class LocalizationModule { @@ -30,7 +31,7 @@ class LocalizationModule public: LocalizationModule(rclcpp::Node * node, const std::string & service_name); - PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose); + std::tuple align_pose(const PoseWithCovarianceStamped & pose); private: rclcpp::Logger logger_; diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 6e023a1309c2d..6535863829fac 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -37,6 +37,8 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); + diagnostics_pose_reliable_ = std::make_unique(this, "pose_initializer_status"); + if (declare_parameter("ekf_enabled")) { ekf_localization_trigger_ = std::make_unique(this); } @@ -151,13 +153,27 @@ void PoseInitializer::on_initialize( auto pose = req->pose_with_covariance.empty() ? get_gnss_pose() : req->pose_with_covariance.front(); + bool reliable = true; if (ndt_) { - pose = ndt_->align_pose(pose); + std::tie(pose, reliable) = ndt_->align_pose(pose); } else if (yabloc_) { // If both the NDT and YabLoc initializer are enabled, prioritize NDT as it offers more // accuracy pose. - pose = yabloc_->align_pose(pose); + std::tie(pose, reliable) = yabloc_->align_pose(pose); + } + + diagnostics_pose_reliable_->clear(); + + // check initial pose result and publish diagnostics + diagnostics_pose_reliable_->add_key_value("initial_pose_reliable", reliable); + if (!reliable) { + std::stringstream message; + message << "Initial Pose Estimation is Unstable."; + diagnostics_pose_reliable_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); } + diagnostics_pose_reliable_->publish(this->now()); + pose.pose.covariance = output_pose_covariance_; pub_reset_->publish(pose); diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index 0a9a0614f6d20..cd82d522187c2 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -15,6 +15,8 @@ #ifndef POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_ +#include "localization_util/diagnostics_module.hpp" + #include #include #include @@ -55,6 +57,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index 6aff744ef4975..23f711d1ba17b 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -199,6 +199,8 @@ void CameraPoseInitializer::on_service( // Estimate orientation const double initial_yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w); const auto yaw_angle_rad_opt = estimate_pose(pos_vec3f, initial_yaw_angle_rad, yaw_std_rad); + // TODO(localization/mapping team): judge reliability of estimate pose + response->reliable = true; if (yaw_angle_rad_opt.has_value()) { response->success = true; response->pose_with_covariance = diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 6cceff98a8f9a..b8f92504a8b99 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,12 +19,12 @@ ament_cmake_auto autoware_cmake + autoware_geography_utils autoware_lanelet2_extension autoware_map_msgs component_interface_specs component_interface_utils fmt - geography_utils geometry_msgs libpcl-all-dev pcl_conversions diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 437c9e44cb7c1..9704b2ec68150 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -36,12 +36,12 @@ #include "lanelet2_local_projector.hpp" #include +#include #include #include #include #include #include -#include #include #include @@ -139,7 +139,7 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( lanelet::ErrorMessages errors{}; if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { std::unique_ptr projector = - geography_utils::get_lanelet2_projector(projector_info); + autoware::geography_utils::get_lanelet2_projector(projector_info); lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); if (errors.empty()) { return map; diff --git a/perception/autoware_bytetrack/lib/include/strack.h b/perception/autoware_bytetrack/lib/include/strack.h index 260f38ea93770..b66005eadce0f 100644 --- a/perception/autoware_bytetrack/lib/include/strack.h +++ b/perception/autoware_bytetrack/lib/include/strack.h @@ -38,8 +38,7 @@ #pragma once -// #include "kalman_filter.h" -#include +#include #include #include @@ -49,6 +48,8 @@ enum TrackState { New = 0, Tracked, Lost, Removed }; +using autoware::kalman_filter::KalmanFilter; + /** manage one tracklet*/ class STrack { diff --git a/perception/autoware_bytetrack/package.xml b/perception/autoware_bytetrack/package.xml index ef2d96e7930d4..581d908b61a51 100644 --- a/perception/autoware_bytetrack/package.xml +++ b/perception/autoware_bytetrack/package.xml @@ -15,12 +15,12 @@ cudnn_cmake_module tensorrt_cmake_module + autoware_kalman_filter autoware_perception_msgs cuda_utils cv_bridge eigen image_transport - kalman_filter libboost-system-dev libopencv-dev rclcpp diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp index e097755c6c506..22027ec6dbd0d 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp @@ -66,6 +66,7 @@ class Debugger pointcloud_within_polygon_->clear(); } + // cppcheck-suppress functionStatic void addNeighborPointcloud(const pcl::PointCloud::Ptr & input) { pcl::PointCloud::Ptr input_xyz = toXYZ(input); diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index cfbb2fa4a18cb..9a0f4838d1fac 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -41,6 +41,7 @@ #include "autoware_perception_msgs/msg/shape.hpp" #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" +#include #include #include @@ -62,6 +63,8 @@ struct PointData size_t orig_index; }; +bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info); + Eigen::Vector2d calcRawImageProjectedPoint( const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d); diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index be14d9a0b0fb3..3d9b51ae7add7 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -271,6 +271,8 @@ void PointPaintingFusionNode::fuseOnSingleImage( return; } + if (!checkCameraInfo(camera_info)) return; + std::vector debug_image_rois; std::vector debug_image_points; diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index d4d5de3cfd381..060b7f292c403 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -83,6 +83,8 @@ void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) { + if (!checkCameraInfo(camera_info)) return; + image_geometry::PinholeCameraModel pinhole_camera_model; pinhole_camera_model.fromCameraInfo(camera_info); diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index e2197c148bcf9..39835b465d8af 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -75,6 +75,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg __attribute__((unused))) { + if (!checkCameraInfo(camera_info)) return; + Eigen::Affine3d object2camera_affine; { const auto transform_stamped_optional = getTransformStamped( diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 40406d6e420b7..e10c7e94f4dae 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -82,6 +82,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (input_pointcloud_msg.data.empty()) { return; } + if (!checkCameraInfo(camera_info)) return; + std::vector output_objs; // select ROIs for fusion for (const auto & feature_obj : input_roi_msg.feature_objects) { diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 5c93fa6c2fd1b..4b4d940a05ab3 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -57,6 +57,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( if (input_pointcloud_msg.data.empty()) { return; } + if (!checkCameraInfo(camera_info)) return; + cv_bridge::CvImagePtr in_image_ptr; try { in_image_ptr = cv_bridge::toCvCopy( diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index ac17c419efca4..1b303e3f01ff5 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -14,8 +14,32 @@ #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include + namespace autoware::image_projection_based_fusion { +bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) +{ + const bool is_supported_model = + (camera_info.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB || + camera_info.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL); + if (!is_supported_model) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("image_projection_based_fusion"), + "checkCameraInfo: Unsupported distortion model: " << camera_info.distortion_model); + return false; + } + const bool is_supported_distortion_param = + (camera_info.d.size() == 5 || camera_info.d.size() == 8); + if (!is_supported_distortion_param) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("image_projection_based_fusion"), + "checkCameraInfo: Unsupported distortion coefficients size: " << camera_info.d.size()); + return false; + } + return true; +} + Eigen::Vector2d calcRawImageProjectedPoint( const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d) { diff --git a/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml index a5b7b0ad6be01..b45cc25f82c1c 100644 --- a/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml @@ -50,3 +50,8 @@ consider_only_routable_neighbours: false reference_path_resolution: 0.5 #[m] + + # debug parameters + publish_processing_time: false + publish_processing_time_detail: false + publish_debug_markers: false diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index b22609100bee1..3078fe89444b8 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ #include #include #include +#include #include #include @@ -54,6 +56,27 @@ #include #include +namespace std +{ +template <> +struct hash +{ + // 0x9e3779b9 is a magic number. See + // https://stackoverflow.com/questions/4948780/magic-number-in-boosthash-combine + size_t operator()(const lanelet::routing::LaneletPaths & paths) const + { + size_t seed = 0; + for (const auto & path : paths) { + for (const auto & lanelet : path) { + seed ^= hash{}(lanelet.id()) + 0x9e3779b9 + (seed << 6U) + (seed >> 2U); + } + // Add a separator between paths + seed ^= hash{}(0) + 0x9e3779b9 + (seed << 6U) + (seed >> 2U); + } + return seed; + } +}; +} // namespace std namespace autoware::map_based_prediction { struct LateralKinematicsToLanelet @@ -229,7 +252,7 @@ class MapBasedPredictionNode : public rclcpp::Node std::unique_ptr published_time_publisher_; rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - mutable autoware::universe_utils::TimeKeeper time_keeper_; + std::shared_ptr time_keeper_; // Member Functions void mapCallback(const LaneletMapBin::ConstSharedPtr msg); @@ -291,7 +314,10 @@ class MapBasedPredictionNode : public rclcpp::Node const float path_probability, const ManeuverProbability & maneuver_probability, const Maneuver & maneuver, std::vector & reference_paths, const double speed_limit = 0.0); - std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); + + mutable universe_utils::LRUCache> + lru_cache_of_convert_path_type_{1000}; + std::vector convertPathType(const lanelet::routing::LaneletPaths & paths) const; void updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths); @@ -317,6 +343,10 @@ class MapBasedPredictionNode : public rclcpp::Node const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); + void publish( + const PredictedObjects & output, + const visualization_msgs::msg::MarkerArray & debug_markers) const; + // NOTE: This function is copied from the motion_velocity_smoother package. // TODO(someone): Consolidate functions and move them to a common inline std::vector calcTrajectoryCurvatureFrom3Points( diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp index bd098894f8a88..8861598ae7fb2 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -16,6 +16,7 @@ #define MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ #include +#include #include #include @@ -82,6 +83,8 @@ class PathGenerator public: PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); + void setTimeKeeper(std::shared_ptr time_keeper_ptr); + PredictedPath generatePathForNonVehicleObject( const TrackedObject & object, const double duration) const; @@ -119,6 +122,8 @@ class PathGenerator bool use_vehicle_acceleration_; double acceleration_exponential_half_life_; + std::shared_ptr time_keeper_; + // Member functions PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const; diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 39c66cd607325..bd8b6557bc3b6 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -52,6 +53,7 @@ namespace autoware::map_based_prediction { +using autoware::universe_utils::ScopedTimeTrack; namespace { @@ -358,42 +360,14 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa back_center_point, r_p_back, l_p_back}; } -bool withinLanelet( - const TrackedObject & object, const lanelet::ConstLanelet & lanelet, - const bool use_yaw_information = false, const float yaw_threshold = 0.6) -{ - using Point = boost::geometry::model::d2::point_xy; - - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - const Point p_object{obj_pos.x, obj_pos.y}; - - auto polygon = lanelet.polygon2d().basicPolygon(); - boost::geometry::correct(polygon); - bool with_in_polygon = boost::geometry::within(p_object, polygon); - - if (!use_yaw_information) return with_in_polygon; - - // use yaw angle to compare - const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); - if (abs_yaw_diff < yaw_threshold) return with_in_polygon; - - return false; -} - bool withinRoadLanelet( - const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const TrackedObject & object, + const std::vector> & surrounding_lanelets_with_dist, const bool use_yaw_information = false) { - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet - constexpr double search_radius = 10.0; // [m] - const auto surrounding_lanelets = - lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius); - - for (const auto & lanelet : surrounding_lanelets) { - if (lanelet.second.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) { + if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype); if ( attr.value() == lanelet::AttributeValueString::Crosswalk || attr.value() == lanelet::AttributeValueString::Walkway) { @@ -401,7 +375,13 @@ bool withinRoadLanelet( } } - if (withinLanelet(object, lanelet.second, use_yaw_information)) { + constexpr float yaw_threshold = 0.6; + bool within_lanelet = std::abs(dist) < 1e-5; + if (use_yaw_information) { + within_lanelet = + within_lanelet && calcAbsYawDiffBetweenLaneletAndObject(object, lanelet) < yaw_threshold; + } + if (within_lanelet) { return true; } } @@ -409,10 +389,24 @@ bool withinRoadLanelet( return false; } +bool withinRoadLanelet( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const bool use_yaw_information = false) +{ + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); + // nearest lanelet + constexpr double search_radius = 10.0; // [m] + const auto surrounding_lanelets_with_dist = + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius); + + return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information); +} + boost::optional isReachableCrosswalkEdgePoints( const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets, - const lanelet::ConstLanelets & external_surrounding_crosswalks, - const CrosswalkEdgePoints & edge_points, const double time_horizon, const double min_object_vel) + const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points, + const double time_horizon, const double min_object_vel) { using Point = boost::geometry::model::d2::point_xy; @@ -439,10 +433,10 @@ boost::optional isReachableCrosswalkEdgePoints( const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - const auto isAcrossAnyRoad = [&surrounding_lanelets, &external_surrounding_crosswalks]( + const auto isAcrossAnyRoad = [&surrounding_lanelets, &surrounding_crosswalks]( const Point & p_src, const Point & p_dst) { - const auto withinAnyCrosswalk = [&external_surrounding_crosswalks](const Point & p) { - for (const auto & crosswalk : external_surrounding_crosswalks) { + const auto withinAnyCrosswalk = [&surrounding_crosswalks](const Point & p) { + for (const auto & crosswalk : surrounding_crosswalks) { if (boost::geometry::within(p, crosswalk.polygon2d().basicPolygon())) { return true; } @@ -831,12 +825,18 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ timeout_set_for_no_intention_to_walk_ = declare_parameter>( "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); + // debug parameter + bool use_time_publisher = declare_parameter("publish_processing_time"); + bool use_time_keeper = declare_parameter("publish_processing_time_detail"); + bool use_debug_marker = declare_parameter("publish_debug_markers"); + path_generator_ = std::make_shared( prediction_sampling_time_interval_, min_crosswalk_user_velocity_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); + // subscribers sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); @@ -844,26 +844,37 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); + // publishers pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); - pub_debug_markers_ = - this->create_publisher("maneuver", rclcpp::QoS{1}); - processing_time_publisher_ = - std::make_unique(this, "map_based_prediction"); - - published_time_publisher_ = - std::make_unique(this); - detailed_processing_time_publisher_ = - this->create_publisher( - "~/debug/processing_time_detail_ms", 1); - time_keeper_ = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + // debug publishers + if (use_time_publisher) { + processing_time_publisher_ = + std::make_unique(this, "map_based_prediction"); + published_time_publisher_ = + std::make_unique(this); + stop_watch_ptr_ = + std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + if (use_time_keeper) { + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); + path_generator_->setTimeKeeper(time_keeper_); + } + + if (use_debug_marker) { + pub_debug_markers_ = + this->create_publisher("maneuver", rclcpp::QoS{1}); + } + // dynamic reconfigure set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); - - stop_watch_ptr_ = - std::make_unique>(); - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); } rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( @@ -916,12 +927,11 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject( void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + lru_cache_of_convert_path_type_.clear(); // clear cache RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Map is loaded"); const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); @@ -944,7 +954,8 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg void MapBasedPredictionNode::trafficSignalsCallback( const TrafficLightGroupArray::ConstSharedPtr msg) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); traffic_signal_id_map_.clear(); for (const auto & signal : msg->traffic_light_groups) { @@ -954,9 +965,10 @@ void MapBasedPredictionNode::trafficSignalsCallback( void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - stop_watch_ptr_->toc("processing_time", true); + if (stop_watch_ptr_) stop_watch_ptr_->toc("processing_time", true); // take traffic_signal { @@ -971,23 +983,6 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt return; } - auto world2map_transform = transform_listener_.getTransform( - "map", // target - in_objects->header.frame_id, // src - in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); - auto map2world_transform = transform_listener_.getTransform( - in_objects->header.frame_id, // target - "map", // src - in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); - auto debug_map2lidar_transform = transform_listener_.getTransform( - "base_link", // target - "map", // src - rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); - - if (!world2map_transform || !map2world_transform || !debug_map2lidar_transform) { - return; - } - // Remove old objects information in object history const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); removeOldObjectsHistory(objects_detected_time, object_buffer_time_length_, road_users_history); @@ -1026,11 +1021,23 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } std::unordered_set predicted_crosswalk_users_ids; + // get world to map transform + geometry_msgs::msg::TransformStamped::ConstSharedPtr world2map_transform; + + bool is_object_not_in_map_frame = in_objects->header.frame_id != "map"; + if (is_object_not_in_map_frame) { + world2map_transform = transform_listener_.getTransform( + "map", // target + in_objects->header.frame_id, // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + if (!world2map_transform) return; + } + for (const auto & object : in_objects->objects) { TrackedObject transformed_object = object; // transform object frame if it's based on map frame - if (in_objects->header.frame_id != "map") { + if (is_object_not_in_map_frame) { geometry_msgs::msg::PoseStamped pose_in_map; geometry_msgs::msg::PoseStamped pose_orig; pose_orig.pose = object.kinematics.pose_with_covariance.pose; @@ -1119,14 +1126,16 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } // Get Debug Marker for On Lane Vehicles - const auto max_prob_path = std::max_element( - ref_paths.begin(), ref_paths.end(), - [](const PredictedRefPath & a, const PredictedRefPath & b) { - return a.probability < b.probability; - }); - const auto debug_marker = - getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); - debug_markers.markers.push_back(debug_marker); + if (pub_debug_markers_) { + const auto max_prob_path = std::max_element( + ref_paths.begin(), ref_paths.end(), + [](const PredictedRefPath & a, const PredictedRefPath & b) { + return a.probability < b.probability; + }); + const auto debug_marker = + getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); + debug_markers.markers.push_back(debug_marker); + } // Fix object angle if its orientation unreliable (e.g. far object by radar sensor) // This prevent bending predicted path @@ -1232,21 +1241,36 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } // Publish Results + publish(output, debug_markers); + + // Publish Processing Time + if (stop_watch_ptr_) { + const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +void MapBasedPredictionNode::publish( + const PredictedObjects & output, const visualization_msgs::msg::MarkerArray & debug_markers) const +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + pub_objects_->publish(output); - published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp); - pub_debug_markers_->publish(debug_markers); - const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); + if (published_time_publisher_) + published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp); + if (pub_debug_markers_) pub_debug_markers_->publish(debug_markers); } void MapBasedPredictionNode::updateCrosswalkUserHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); CrosswalkUserData crosswalk_user_data; crosswalk_user_data.header = header; @@ -1263,7 +1287,8 @@ void MapBasedPredictionNode::updateCrosswalkUserHistory( std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( const std::string & object_id, std::unordered_map & current_users) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const auto known_match_opt = [&]() -> std::optional { if (!known_matches_.count(object_id)) { @@ -1322,7 +1347,8 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); lanelet::BasicLineString2d predicted_path_ls; for (const auto & p : predicted_path.path) @@ -1340,7 +1366,8 @@ bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predict bool MapBasedPredictionNode::doesPathCrossFence( const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // check whether the predicted path cross with fence for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) { @@ -1359,6 +1386,9 @@ bool MapBasedPredictionNode::isIntersecting( const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + const auto p1 = autoware::universe_utils::createPoint(point1.x, point1.y, 0.0); const auto p2 = autoware::universe_utils::createPoint(point2.x, point2.y, 0.0); const auto p3 = autoware::universe_utils::createPoint(point3.x(), point3.y(), 0.0); @@ -1370,7 +1400,8 @@ bool MapBasedPredictionNode::isIntersecting( PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const TrackedObject & object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); auto predicted_object = convertToPredictedObject(object); { @@ -1386,13 +1417,11 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity); - // TODO(Mamoru Sobue): 3rd argument of findNearest is the number of lanelets, not radius, so past - // implementation has been wrong. - const auto surrounding_lanelets_with_dist = lanelet::geometry::findNearest( + const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d( lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y}, prediction_time_horizon_.pedestrian * velocity); lanelet::ConstLanelets surrounding_lanelets; - lanelet::ConstLanelets external_surrounding_crosswalks; + lanelet::ConstLanelets surrounding_crosswalks; for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) { surrounding_lanelets.push_back(lanelet); const auto attr = lanelet.attribute(lanelet::AttributeName::Subtype); @@ -1400,10 +1429,9 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( attr.value() == lanelet::AttributeValueString::Crosswalk || attr.value() == lanelet::AttributeValueString::Walkway) { const auto & crosswalk = lanelet; - if (withinLanelet(object, crosswalk)) { + surrounding_crosswalks.push_back(crosswalk); + if (std::abs(dist) < 1e-5) { crossing_crosswalk = crosswalk; - } else { - external_surrounding_crosswalks.push_back(crosswalk); } } } @@ -1434,7 +1462,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest // crosswalk and generate path to the crosswalk edge - } else if (withinRoadLanelet(object, lanelet_map_ptr_)) { + } else if (withinRoadLanelet(object, surrounding_lanelets_with_dist)) { lanelet::ConstLanelet closest_crosswalk{}; const auto & obj_pose = object.kinematics.pose_with_covariance.pose; const auto found_closest_crosswalk = @@ -1467,7 +1495,10 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( // try to find the edge points for other surrounding crosswalks and generate path to the crosswalk // edge - for (const auto & crosswalk : external_surrounding_crosswalks) { + for (const auto & crosswalk : surrounding_crosswalks) { + if (crossing_crosswalk && crossing_crosswalk.get() == crosswalk) { + continue; + } const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { if (!calcIntentionToCrossWithTrafficSignal( @@ -1492,7 +1523,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, surrounding_lanelets, external_surrounding_crosswalks, edge_points, + object, surrounding_lanelets, surrounding_crosswalks, edge_points, prediction_time_horizon_.pedestrian, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { @@ -1524,7 +1555,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( void MapBasedPredictionNode::updateObjectData(TrackedObject & object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); if ( object.kinematics.orientation_availability == @@ -1576,8 +1608,6 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) void MapBasedPredictionNode::removeStaleTrafficLightInfo( const TrackedObjects::ConstSharedPtr in_objects) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) { const bool isDisappeared = std::none_of( in_objects->objects.begin(), in_objects->objects.end(), @@ -1594,7 +1624,8 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo( LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // obstacle point lanelet::BasicPoint2d search_point( @@ -1686,7 +1717,8 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob bool MapBasedPredictionNode::checkCloseLaneletCondition( const std::pair & lanelet, const TrackedObject & object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { @@ -1734,7 +1766,8 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( float MapBasedPredictionNode::calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const auto & obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -1771,7 +1804,8 @@ void MapBasedPredictionNode::updateRoadUsersHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::string object_id = autoware::universe_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); @@ -1814,7 +1848,8 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, const double object_detected_time, const double time_horizon) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -1978,7 +2013,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // calculate maneuver const auto current_maneuver = [&]() { @@ -2030,7 +2066,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( const TrackedObject & object, const LaneletData & current_lanelet_data, const double /*object_detected_time*/) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // Step1. Check if we have the object in the buffer const std::string object_id = autoware::universe_utils::toHexString(object.object_id); @@ -2103,7 +2140,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( const TrackedObject & object, const LaneletData & current_lanelet_data, const double /*object_detected_time*/) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // Step1. Check if we have the object in the buffer const std::string object_id = autoware::universe_utils::toHexString(object.object_id); @@ -2248,8 +2286,6 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( double MapBasedPredictionNode::calcRightLateralOffset( const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - std::vector boundary_path(boundary_line.size()); for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); @@ -2269,7 +2305,8 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { @@ -2295,7 +2332,8 @@ void MapBasedPredictionNode::addReferencePaths( const Maneuver & maneuver, std::vector & reference_paths, const double speed_limit) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); if (!candidate_paths.empty()) { updateFuturePossibleLanelets(object, candidate_paths); @@ -2316,7 +2354,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( const lanelet::routing::LaneletPaths & right_paths, const lanelet::routing::LaneletPaths & center_paths) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); float left_lane_change_probability = 0.0; float right_lane_change_probability = 0.0; @@ -2378,9 +2417,14 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( } std::vector MapBasedPredictionNode::convertPathType( - const lanelet::routing::LaneletPaths & paths) + const lanelet::routing::LaneletPaths & paths) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + if (lru_cache_of_convert_path_type_.contains(paths)) { + return *lru_cache_of_convert_path_type_.get(paths); + } std::vector converted_paths; for (const auto & path : paths) { @@ -2465,6 +2509,7 @@ std::vector MapBasedPredictionNode::convertPathType( converted_paths.push_back(resampled_converted_path); } + lru_cache_of_convert_path_type_.put(paths, converted_paths); return converted_paths; } @@ -2472,8 +2517,6 @@ bool MapBasedPredictionNode::isDuplicated( const std::pair & target_lanelet, const LaneletsData & lanelets_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - const double CLOSE_LANELET_THRESHOLD = 0.1; for (const auto & lanelet_data : lanelets_data) { const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); @@ -2491,8 +2534,6 @@ bool MapBasedPredictionNode::isDuplicated( bool MapBasedPredictionNode::isDuplicated( const PredictedPath & predicted_path, const std::vector & predicted_paths) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - const double CLOSE_PATH_THRESHOLD = 0.1; for (const auto & prev_predicted_path : predicted_paths) { const auto prev_path_end = prev_predicted_path.path.back().position; @@ -2509,8 +2550,6 @@ bool MapBasedPredictionNode::isDuplicated( std::optional MapBasedPredictionNode::getTrafficSignalId( const lanelet::ConstLanelet & way_lanelet) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - const auto traffic_light_reg_elems = way_lanelet.regulatoryElementsAs(); if (traffic_light_reg_elems.empty()) { @@ -2527,7 +2566,8 @@ std::optional MapBasedPredictionNode::getTrafficSignalId( std::optional MapBasedPredictionNode::getTrafficSignalElement( const lanelet::Id & id) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); if (traffic_signal_id_map_.count(id) != 0) { const auto & signal_elements = traffic_signal_id_map_.at(id).elements; @@ -2545,7 +2585,8 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, const lanelet::Id & signal_id) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const auto signal_color = [&] { const auto elem_opt = getTrafficSignalElement(signal_id); diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 2dc7b9f4f95ee..0d8b7c6f6cd20 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -22,6 +22,8 @@ namespace autoware::map_based_prediction { +using autoware::universe_utils::ScopedTimeTrack; + PathGenerator::PathGenerator( const double sampling_time_interval, const double min_crosswalk_user_velocity) : sampling_time_interval_(sampling_time_interval), @@ -29,15 +31,27 @@ PathGenerator::PathGenerator( { } +void PathGenerator::setTimeKeeper( + std::shared_ptr time_keeper_ptr) +{ + time_keeper_ = std::move(time_keeper_ptr); +} + PredictedPath PathGenerator::generatePathForNonVehicleObject( const TrackedObject & object, const double duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PredictedPath predicted_path{}; const double ep = 0.001; @@ -77,6 +91,9 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, const double duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PredictedPath predicted_path{}; const double ep = 0.001; @@ -135,6 +152,9 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( PredictedPath PathGenerator::generatePathForLowSpeedVehicle( const TrackedObject & object, const double duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); const double ep = 0.001; @@ -148,6 +168,9 @@ PredictedPath PathGenerator::generatePathForLowSpeedVehicle( PredictedPath PathGenerator::generatePathForOffLaneVehicle( const TrackedObject & object, const double duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return generateStraightPath(object, duration); } @@ -155,6 +178,9 @@ PredictedPath PathGenerator::generatePathForOnLaneVehicle( const TrackedObject & object, const PosePath & ref_paths, const double duration, const double lateral_duration, const double speed_limit) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (ref_paths.size() < 2) { return generateStraightPath(object, duration); } @@ -186,6 +212,9 @@ PredictedPath PathGenerator::generatePolynomialPath( const TrackedObject & object, const PosePath & ref_path, const double duration, const double lateral_duration, const double speed_limit) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + // Get current Frenet Point const double ref_path_len = autoware::motion_utils::calcArcLength(ref_path); const auto current_point = getFrenetPoint(object, ref_path, duration, speed_limit); @@ -219,6 +248,9 @@ FrenetPath PathGenerator::generateFrenetPath( const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, const double duration, const double lateral_duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + FrenetPath path; // Compute Lateral and Longitudinal Coefficients to generate the trajectory @@ -303,6 +335,9 @@ Eigen::Vector2d PathGenerator::calcLonCoefficients( PosePath PathGenerator::interpolateReferencePath( const PosePath & base_path, const FrenetPath & frenet_predicted_path) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PosePath interpolated_path; const size_t interpolate_num = frenet_predicted_path.size(); if (interpolate_num < 2) { @@ -361,6 +396,9 @@ PredictedPath PathGenerator::convertToPredictedPath( const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PredictedPath predicted_path; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); predicted_path.path.resize(ref_path.size()); @@ -392,6 +430,9 @@ FrenetPoint PathGenerator::getFrenetPoint( const TrackedObject & object, const PosePath & ref_path, const double duration, const double speed_limit) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 5374e28d5f9cf..7cb2963d38ef1 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -19,10 +19,10 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" -#include "kalman_filter/kalman_filter.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index c3f824aff35b4..227e6cd01f4dc 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -19,10 +19,10 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" -#include "kalman_filter/kalman_filter.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index a0a6bd7781761..868aa1606d4ff 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -19,10 +19,10 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "kalman_filter/kalman_filter.hpp" #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index f7edebfc31378..8f5bab65c6aed 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -19,10 +19,10 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" -#include "kalman_filter/kalman_filter.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index e7f0a5fd699e1..45cd0f31a4e85 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -19,7 +19,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#include "kalman_filter/kalman_filter.hpp" +#include "autoware/kalman_filter/kalman_filter.hpp" #include "tracker_base.hpp" namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index f593280c2e183..4287e0f99d5ee 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -19,10 +19,10 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "kalman_filter/kalman_filter.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 1e0f778a69137..500148ba41081 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -19,10 +19,10 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" -#include "kalman_filter/kalman_filter.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index 4bc03f439ffc2..9f128c864ad6c 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,9 +19,9 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" -#include "kalman_filter/kalman_filter.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 6e20d31aad168..123eb30e63d6c 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -19,8 +19,8 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "kalman_filter/kalman_filter.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 3cca786d9f65b..2632d99047053 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -19,8 +19,8 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "kalman_filter/kalman_filter.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index ad3061285a80c..26799f1916741 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -19,8 +19,8 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "kalman_filter/kalman_filter.hpp" #include #include diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp index 130053fafd2ed..dff60a82c4c44 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -19,7 +19,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ -#include "kalman_filter/kalman_filter.hpp" +#include "autoware/kalman_filter/kalman_filter.hpp" #include #include @@ -33,6 +33,7 @@ namespace autoware::multi_object_tracker { +using autoware::kalman_filter::KalmanFilter; class MotionModel { diff --git a/perception/autoware_multi_object_tracker/package.xml b/perception/autoware_multi_object_tracker/package.xml index 3d941239976ea..405ce7eced625 100644 --- a/perception/autoware_multi_object_tracker/package.xml +++ b/perception/autoware_multi_object_tracker/package.xml @@ -13,12 +13,12 @@ autoware_cmake eigen3_cmake_module + autoware_kalman_filter autoware_perception_msgs autoware_universe_utils diagnostic_updater eigen glog - kalman_filter mussp object_recognition_utils rclcpp diff --git a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json index 96f612828b597..3d7bba9daae87 100644 --- a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json +++ b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json @@ -8,22 +8,26 @@ "properties": { "topic": { "type": "string", - "description": "The ROS topic name for the input channel." + "description": "The ROS topic name for the input channel.", + "default": "/perception/object_recognition/detection/objects" }, "can_spawn_new_tracker": { "type": "boolean", - "description": "Indicates if the input channel can spawn new trackers." + "description": "Indicates if the input channel can spawn new trackers.", + "default": true }, "optional": { "type": "object", "properties": { "name": { "type": "string", - "description": "The name of the input channel." + "description": "The name of the input channel.", + "default": "detected_objects" }, "short_name": { "type": "string", - "description": "The short name of the input channel." + "description": "The short name of the input channel.", + "default": "all" } } } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 89d8cbaeb3bd8..4b3240d81e68f 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -209,6 +209,12 @@ void MultiObjectTracker::onTrigger() const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; onMessage(objects_list); + + // Publish without delay compensation + if (!publish_timer_) { + const auto latest_object_time = rclcpp::Time(objects_list.back().second.header.stamp); + checkAndPublish(latest_object_time); + } } void MultiObjectTracker::onTimer() @@ -222,7 +228,7 @@ void MultiObjectTracker::onTimer() onMessage(objects_list); } - // Publish + // Publish with delay compensation checkAndPublish(current_time); } diff --git a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 25e174a44c2ec..98f429292ec73 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -271,8 +271,8 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( // calculate twist for radar data with target_value * average Eigen::Vector2d vec_target_value_average(0.0, 0.0); - double sum_target_value = 0.0; if (param_.velocity_weight_target_value_average > 0.0) { + double sum_target_value = 0.0; for (const auto & radar : (*radars)) { vec_target_value_average += (toVector2d(radar.twist_with_covariance) * radar.target_value); sum_target_value += radar.target_value; diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp index dab4d3f8efa24..6b33552c65891 100644 --- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp +++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp @@ -15,13 +15,14 @@ #ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ #define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp" -#include "kalman_filter/kalman_filter.hpp" #include namespace autoware::radar_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; +using autoware::kalman_filter::KalmanFilter; class ConstantTurnRateMotionTracker : public Tracker // means constant turn rate motion tracker { private: diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp index 7c96aa8fbaa36..b73e064635974 100644 --- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp +++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ #define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_ +#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp" -#include "kalman_filter/kalman_filter.hpp" #include @@ -24,6 +24,8 @@ namespace autoware::radar_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; +using autoware::kalman_filter::KalmanFilter; + class LinearMotionTracker : public Tracker { private: diff --git a/perception/autoware_radar_object_tracker/package.xml b/perception/autoware_radar_object_tracker/package.xml index 50dd70e6cec67..6829c26f797c7 100644 --- a/perception/autoware_radar_object_tracker/package.xml +++ b/perception/autoware_radar_object_tracker/package.xml @@ -13,12 +13,12 @@ autoware_cmake eigen3_cmake_module + autoware_kalman_filter autoware_lanelet2_extension autoware_perception_msgs autoware_universe_utils eigen glog - kalman_filter mussp nlohmann-json-dev object_recognition_utils diff --git a/perception/autoware_tensorrt_yolox/CMakeLists.txt b/perception/autoware_tensorrt_yolox/CMakeLists.txt index 564ac54dbc563..1f54326f2b33a 100644 --- a/perception/autoware_tensorrt_yolox/CMakeLists.txt +++ b/perception/autoware_tensorrt_yolox/CMakeLists.txt @@ -143,7 +143,6 @@ rclcpp_components_register_node(yolox_single_image_inference_node ) ament_auto_add_library(${PROJECT_NAME}_node SHARED - src/utils.cpp src/tensorrt_yolox_node.cpp ) diff --git a/perception/autoware_tensorrt_yolox/package.xml b/perception/autoware_tensorrt_yolox/package.xml index 9205b0c83cbea..3373a07d8b434 100644 --- a/perception/autoware_tensorrt_yolox/package.xml +++ b/perception/autoware_tensorrt_yolox/package.xml @@ -24,6 +24,7 @@ image_transport libopencv-dev object_recognition_utils + perception_utils rclcpp rclcpp_components sensor_msgs diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp index 4b12a8e19c31f..675fee64812a2 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp @@ -772,8 +772,6 @@ void TrtYoloX::multiScalePreprocess(const cv::Mat & image, const std::vector & images, ObjectArrays & objects, const std::vector & rois) { - std::vector masks; - std::vector color_masks; if (!trt_common_->isInitialized()) { return false; } @@ -784,6 +782,8 @@ bool TrtYoloX::doInferenceWithRoi( } if (needs_output_decode_) { + std::vector masks; + std::vector color_masks; return feedforwardAndDecode(images, objects, masks, color_masks); } else { return feedforward(images, objects); diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp index 7f398ca005d67..c613e7d1df52f 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -14,8 +14,8 @@ #include "autoware/tensorrt_yolox/tensorrt_yolox_node.hpp" -#include "autoware/tensorrt_yolox/utils.hpp" #include "object_recognition_utils/object_classification.hpp" +#include "perception_utils/run_length_encoder.hpp" #include @@ -184,7 +184,7 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) .toImageMsg(); out_mask_msg->header = msg->header; - std::vector> compressed_data = runLengthEncoder(mask); + std::vector> compressed_data = perception_utils::runLengthEncoder(mask); int step = sizeof(uint8_t) + sizeof(int); out_mask_msg->data.resize(static_cast(compressed_data.size()) * step); for (size_t i = 0; i < compressed_data.size(); ++i) { diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp index 7482749630a48..7ef13cf457c07 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp @@ -20,7 +20,6 @@ #include #include -#include namespace autoware::traffic_light { @@ -103,26 +102,18 @@ bool TrafficLightRoiVisualizerNode::createRect( cv::Mat & image, const tier4_perception_msgs::msg::TrafficLightRoi & tl_roi, const ClassificationResult & result) { - cv::Scalar color; - if (result.label.find("red") != std::string::npos) { - color = cv::Scalar{254, 149, 149}; - } else if (result.label.find("yellow") != std::string::npos) { - color = cv::Scalar{254, 250, 149}; - } else if (result.label.find("green") != std::string::npos) { - color = cv::Scalar{149, 254, 161}; - } else { - color = cv::Scalar{250, 250, 250}; - } + const auto info = extractShapeInfo(result.label); cv::rectangle( image, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), cv::Point(tl_roi.roi.x_offset + tl_roi.roi.width, tl_roi.roi.y_offset + tl_roi.roi.height), - color, 2); + info.color, 2); - std::string shape_name = extractShapeName(result.label); + constexpr int shape_img_size = 16; + const auto position = cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset); visualization::drawTrafficLightShape( - image, shape_name, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), color, 16, result.prob); + image, info.shapes, shape_img_size, position, info.color, result.prob); return true; } diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp index 47a13389950a5..245c6caa6946c 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp @@ -36,7 +36,9 @@ #include #include #include +#include #include +#include namespace autoware::traffic_light { @@ -46,6 +48,15 @@ struct ClassificationResult std::string label; }; +/** + * @brief A struct to represent parsed traffic light shape information. + */ +struct TrafficLightShapeInfo +{ + cv::Scalar color; //!< Color associated with "circle". + std::vector shapes; //!< Shape names. +}; + class TrafficLightRoiVisualizerNode : public rclcpp::Node { public: @@ -78,6 +89,8 @@ class TrafficLightRoiVisualizerNode : public rclcpp::Node {tier4_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW, "right"}, {tier4_perception_msgs::msg::TrafficLightElement::UP_ARROW, "straight"}, {tier4_perception_msgs::msg::TrafficLightElement::DOWN_ARROW, "down"}, + {tier4_perception_msgs::msg::TrafficLightElement::UP_LEFT_ARROW, "straight_left"}, + {tier4_perception_msgs::msg::TrafficLightElement::UP_RIGHT_ARROW, "straight_right"}, {tier4_perception_msgs::msg::TrafficLightElement::DOWN_LEFT_ARROW, "down_left"}, {tier4_perception_msgs::msg::TrafficLightElement::DOWN_RIGHT_ARROW, "down_right"}, {tier4_perception_msgs::msg::TrafficLightElement::CROSS, "cross"}, @@ -85,18 +98,48 @@ class TrafficLightRoiVisualizerNode : public rclcpp::Node {tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN, "unknown"}, }; - std::string extractShapeName(const std::string & label) + /** + * @brief Return RGB color from color string associated with "circle". + * @param color Color string. + * @return RGB color. + */ + static cv::Scalar strToColor(const std::string & color) + { + if (color == "red") { + return {254, 149, 149}; + } else if (color == "yellow") { + return {254, 250, 149}; + } else if (color == "green") { + return {149, 254, 161}; + } else { + return {250, 250, 250}; + } + } + + /** + * @brief Extract color and shape names from label. + * @param label String formatted as `-,-,...,-`. + * @return Extracted information includes a color associated with "circle" and shape names. + */ + static TrafficLightShapeInfo extractShapeInfo(const std::string & label) { - size_t start_pos = label.find('-'); - if (start_pos != std::string::npos) { - start_pos++; // Start after the hyphen - size_t end_pos = label.find(',', start_pos); // Find the next comma after the hyphen - if (end_pos == std::string::npos) { // If no comma is found, take the rest of the string - end_pos = label.length(); + cv::Scalar color{255, 255, 255}; + std::vector shapes; + + std::stringstream ss(label); + std::string segment; + while (std::getline(ss, segment, ',')) { + size_t hyphen_pos = segment.find('-'); + if (hyphen_pos != std::string::npos) { + auto shape = segment.substr(hyphen_pos + 1); + if (shape == "circle") { + const auto color_str = segment.substr(0, hyphen_pos); + color = strToColor(color_str); + } + shapes.emplace_back(shape); } - return label.substr(start_pos, end_pos - start_pos); } - return "unknown"; // Return "unknown" if no hyphen is found + return {color, shapes}; } bool createRect( diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp index c685bc0639601..8c7a59cbcb003 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp @@ -17,164 +17,128 @@ #include "opencv2/highgui.hpp" #include "opencv2/imgproc.hpp" +#include +#include +#include +#include +#include + namespace autoware::traffic_light::visualization { void drawShape( - const DrawFunctionParams & params, const std::string & filename, bool flipHorizontally, - bool flipVertically, int x_offset, int y_offset, double scale_factor) + cv::Mat & image, const std::vector & params, int size, const cv::Point & position, + const cv::Scalar & color, float probability) { - std::string filepath = - ament_index_cpp::get_package_share_directory("autoware_traffic_light_visualization") + - "/images/" + filename; - cv::Mat shapeImg = cv::imread(filepath, cv::IMREAD_UNCHANGED); - if (shapeImg.empty()) { - std::cerr << "Failed to load image: " << filepath << std::endl; - return; - } + // load concatenated shape image + const auto shape_img = loadShapeImage(params, size); - if (flipHorizontally) { - cv::flip(shapeImg, shapeImg, 1); // Flip horizontally - } + // Calculate the width of the text + std::string prob_str = std::to_string(static_cast(round(probability * 100))) + "%"; - if (flipVertically) { - cv::flip(shapeImg, shapeImg, 0); // Flip vertically - } + int baseline = 0; + cv::Size text_size = cv::getTextSize(prob_str, cv::FONT_HERSHEY_SIMPLEX, 0.75, 2, &baseline); - cv::resize( - shapeImg, shapeImg, cv::Size(params.size, params.size), scale_factor, scale_factor, - cv::INTER_AREA); + const int fill_rect_w = shape_img.cols + text_size.width + 20; + const int fill_rect_h = std::max(shape_img.rows, text_size.height) + 10; - // Calculate the center position including offsets - cv::Point position( - params.position.x + x_offset, params.position.y - shapeImg.rows / 2 + y_offset); + const cv::Point rect_position(position.x, position.y - fill_rect_h); - // Check for image boundaries if ( - position.x < 0 || position.y < 0 || position.x + shapeImg.cols > params.image.cols || - position.y + shapeImg.rows > params.image.rows) { + rect_position.x < 0 || rect_position.y < 0 || rect_position.x + fill_rect_w > image.cols || + position.y > image.rows) { // TODO(KhalilSelyan): This error message may flood the terminal logs, so commented out // temporarily. Need to consider a better way. // std::cerr << "Adjusted position is out of image bounds." << std::endl; return; } - - // Calculate the width of the text - std::string probabilityText = - std::to_string(static_cast(round(params.probability * 100))) + "%"; - int baseline = 0; - cv::Size textSize = - cv::getTextSize(probabilityText, cv::FONT_HERSHEY_SIMPLEX, 0.75, 2, &baseline); - - // Adjust the filled rectangle to be at the top edge and the correct width - int filledRectWidth = - shapeImg.cols + (filename != "unknown.png" ? textSize.width + 10 : 5); // Add some padding - int filledRectHeight = shapeImg.rows + 10; // Add some padding - - cv::rectangle( - params.image, cv::Rect(position.x - 2, position.y - 5, filledRectWidth, filledRectHeight), - params.color, - -1); // Filled rectangle - - // Create ROI on the destination image - cv::Mat destinationROI = params.image(cv::Rect(position, cv::Size(shapeImg.cols, shapeImg.rows))); - - // Overlay the image onto the main image - for (int y = 0; y < shapeImg.rows; ++y) { - for (int x = 0; x < shapeImg.cols; ++x) { - cv::Vec4b & pixel = shapeImg.at(y, x); - if (pixel[3] != 0) { // Only non-transparent pixels - destinationROI.at(y, x) = cv::Vec3b(pixel[0], pixel[1], pixel[2]); + cv::Rect rectangle(rect_position.x, rect_position.y, fill_rect_w, fill_rect_h); + cv::rectangle(image, rectangle, color, -1); + + // Position the probability text right next to the shape. (Text origin: bottom-left) + const int prob_x_offset = shape_img.cols + 15; + const int prob_y_offset = fill_rect_h / 4; + cv::putText( + image, prob_str, cv::Point(position.x + prob_x_offset, position.y - prob_y_offset), + cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0), 2, cv::LINE_AA); + + if (!shape_img.empty()) { + // Create ROI on the destination image + const int shape_y_offset = fill_rect_h / 4; + auto shapeRoi = image(cv::Rect( + rect_position.x + 5, rect_position.y + shape_y_offset, shape_img.cols, shape_img.rows)); + + // Overlay the image onto the main image + for (int y = 0; y < shape_img.rows; ++y) { + for (int x = 0; x < shape_img.cols; ++x) { + const auto & pixel = shape_img.at(y, x); + if (pixel[3] != 0) { // Only non-transparent pixels + shapeRoi.at(y, x) = cv::Vec3b(pixel[0], pixel[1], pixel[2]); + } } } } - - // Position the probability text right next to the shape - if (filename != "unknown.png") { - cv::putText( - params.image, probabilityText, - cv::Point( - position.x + shapeImg.cols + 5, position.y + shapeImg.rows / 2 + textSize.height / 2), - cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0), 2, cv::LINE_AA); - } -} - -void drawCircle(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "circle.png", false, false, 0, -y_offset); -} - -void drawLeftArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "left_arrow.png", false, false, 0, -y_offset); -} - -void drawRightArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "left_arrow.png", true, false, 0, -y_offset); } -void drawStraightArrow(const DrawFunctionParams & params) +cv::Mat loadShapeImage(const std::vector & params, int size, double scale_factor) { - int y_offset = params.size / 2 + 5; // This adjusts the base position upwards + if (params.empty()) { + return {}; + } - drawShape(params, "straight_arrow.png", false, false, 0, -y_offset); -} -void drawDownArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; // This adjusts the base position upwards - drawShape(params, "straight_arrow.png", false, true, 0, -y_offset); -} + static const auto img_dir = + ament_index_cpp::get_package_share_directory("autoware_traffic_light_visualization") + + "/images/"; -void drawDownLeftArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "down_left_arrow.png", false, false, 0, -y_offset); -} + std::vector src_img; + for (const auto & param : params) { + auto filepath = img_dir + param.filename; + auto img = cv::imread(filepath, cv::IMREAD_UNCHANGED); -void drawDownRightArrow(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "down_left_arrow.png", true, false, 0, -y_offset); -} + cv::resize(img, img, cv::Size(size, size), scale_factor, scale_factor, cv::INTER_AREA); -void drawCross(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; + if (param.h_flip) { + cv::flip(img, img, 1); + } + if (param.v_flip) { + cv::flip(img, img, 0); + } + src_img.emplace_back(img); + } - drawShape(params, "cross.png", false, false, 0, -y_offset); -} + cv::Mat dst; + cv::hconcat(src_img, dst); // cspell:ignore hconcat -void drawUnknown(const DrawFunctionParams & params) -{ - int y_offset = params.size / 2 + 5; - drawShape(params, "unknown.png", false, false, 0, -y_offset); + return dst; } void drawTrafficLightShape( - cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color, - int size, float probability) + cv::Mat & image, const std::vector & shapes, int size, const cv::Point & position, + const cv::Scalar & color, float probability) { - static std::map shapeToFunction = { - {"circle", drawCircle}, - {"left", drawLeftArrow}, - {"right", drawRightArrow}, - {"straight", drawStraightArrow}, - {"down", drawDownArrow}, - {"down_left", drawDownLeftArrow}, - {"down_right", drawDownRightArrow}, - {"cross", drawCross}, - {"unknown", drawUnknown}}; - auto it = shapeToFunction.find(shape); - if (it != shapeToFunction.end()) { - DrawFunctionParams params{image, position, color, size, probability}; - it->second(params); - } else { - std::cerr << "Unknown shape: " << shape << std::endl; + using ShapeImgParamFunction = std::function; + + static const std::unordered_map shapeToParamFunction = { + {"circle", circleImgParam}, + {"left", leftArrowImgParam}, + {"right", rightArrowImgParam}, + {"straight", straightArrowImgParam}, + {"down", downArrowImgParam}, + {"straight_left", straightLeftArrowImgParam}, + {"straight_right", straightRightArrowImgParam}, + {"down_left", downLeftArrowImgParam}, + {"down_right", downRightArrowImgParam}, + {"cross", crossImgParam}, + {"unknown", unknownImgParam}}; + + std::vector params; + for (const auto & shape : shapes) { + if (shapeToParamFunction.find(shape) != shapeToParamFunction.end()) { + auto func = shapeToParamFunction.at(shape); + params.emplace_back(func()); + } } -} + drawShape(image, params, size, position, color, probability); +} } // namespace autoware::traffic_light::visualization diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp index 150d78f767ee3..93aae041ede11 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp @@ -20,36 +20,167 @@ #include #include -#include #include +#include namespace autoware::traffic_light::visualization { -struct DrawFunctionParams +/** + * @brief A struct of parameters to load shape image. + */ +struct ShapeImgParam { - cv::Mat & image; - cv::Point position; - cv::Scalar color; - int size; - float probability; + std::string filename; //!< Filename of shape image. + bool h_flip; //!< Whether to flip horizontally + bool v_flip; //!< Whether to flip vertically }; -using DrawFunction = std::function; - +/** + * @brief Draw traffic light shapes on the camera view image. + * @param image Camera view image. + * @param params Shape parameters to load shape image. + * @param size Shape image size to resize. + * @param position Top-left position of a ROI. + * @param color Rectangle color. + * @param probability Classification probability. + */ void drawShape( - const DrawFunctionParams & params, const std::string & filename, bool flipHorizontally, - bool flipVertically, int x_offset, int y_offset, double scale_factor = 0.3); -void drawCircle(const DrawFunctionParams & params); -void drawLeftArrow(const DrawFunctionParams & params); -void drawRightArrow(const DrawFunctionParams & params); -void drawStraightArrow(const DrawFunctionParams & params); -void drawDownArrow(const DrawFunctionParams & params); -void drawDownLeftArrow(const DrawFunctionParams & params); -void drawDownRightArrow(const DrawFunctionParams & params); -void drawCross(const DrawFunctionParams & params); -void drawUnknown(const DrawFunctionParams & params); + cv::Mat & image, const std::vector & params, int size, const cv::Point & position, + const cv::Scalar & color, float probability); + +/** + * @brief Load shape images and concatenate them. + * @param params Parameters for each shape image. + * @param size Image size to resize. + * @param scale_factor Scale factor to resize. + * @return If no parameter is specified returns empty Mat, otherwise returns horizontally + * concatenated image. + */ +cv::Mat loadShapeImage( + const std::vector & params, int size, double scale_factor = 0.3); + +/** + * @brief Load parameter of circle. + * + * @return Parameter of circle. + */ +inline ShapeImgParam circleImgParam() +{ + return {"circle.png", false, false}; +} + +/** + * @brief Load parameter of left-arrow. + * + * @return Parameter of left-arrow. + */ +inline ShapeImgParam leftArrowImgParam() +{ + return {"left_arrow.png", false, false}; +} + +/** + * @brief Load parameter of right-arrow. + * + * @return Parameter of right-arrow, the image is flipped left-arrow horizontally. + */ +inline ShapeImgParam rightArrowImgParam() +{ + return {"left_arrow.png", true, false}; +} + +/** + * @brief Load parameter of straight-arrow. + * + * @return Parameter of straight-arrow. + */ +inline ShapeImgParam straightArrowImgParam() +{ + return {"straight_arrow.png", false, false}; +} + +/** + * @brief Load parameter of down-arrow. + * + * @return Parameter of down-arrow, the image is flipped straight-arrow vertically. + */ +inline ShapeImgParam downArrowImgParam() +{ + return {"straight_arrow.png", false, true}; +} + +/** + * @brief Load parameter of straight-left-arrow. + * + * @return Parameter of straight-left-arrow, the image is flipped down-left-arrow vertically. + */ +inline ShapeImgParam straightLeftArrowImgParam() +{ + return {"down_left_arrow.png", false, true}; +} + +/** + * @brief Load parameter of straight-right-arrow. + * + * @return Parameter of straight-right-arrow, the image is flipped down-left-arrow both horizontally + * and vertically. + */ +inline ShapeImgParam straightRightArrowImgParam() +{ + return {"down_left_arrow.png", true, true}; +} + +/** + * @brief Load parameter of down-left-arrow. + * + * @return Parameter of down-left-arrow. + */ +inline ShapeImgParam downLeftArrowImgParam() +{ + return {"down_left_arrow.png", false, false}; +} + +/** + * @brief Load parameter of down-right-arrow. + * + * @return Parameter of down-right-arrow, the image is flipped straight-arrow horizontally. + */ +inline ShapeImgParam downRightArrowImgParam() +{ + return {"down_left_arrow.png", true, false}; +} + +/** + * @brief Load parameter of cross-arrow. + * + * @return Parameter of cross-arrow. + */ +inline ShapeImgParam crossImgParam() +{ + return {"cross.png", false, false}; +} + +/** + * @brief Load parameter of unknown shape. + * + * @return Parameter of unkown shape. + */ +inline ShapeImgParam unknownImgParam() +{ + return {"unknown.png", false, false}; +} + +/** + * @brief Draw traffic light shapes on the camera view image. + * @param image Camera view image. + * @param shapes Shape names. + * @param size Shape image size to resize. + * @param position Top-left position of a ROI. + * @param color Color of traffic light. + * @param probability Classification probability. + */ void drawTrafficLightShape( - cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color, - int size, float probability); + cv::Mat & image, const std::vector & shapes, int size, const cv::Point & position, + const cv::Scalar & color, float probability); } // namespace autoware::traffic_light::visualization diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index e0fd41a70cd36..4783cf410f0e4 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -38,6 +38,8 @@ None ### Parameters +{{json_to_markdown("planning/autoware_freespace_planner/schema/freespace_planner.schema.json")}} + #### Node parameters | Parameter | Type | Description | @@ -76,9 +78,11 @@ None | --------------------------- | ------ | ------------------------------------------------------- | | `only_behind_solutions` | bool | whether restricting the solutions to be behind the goal | | `use_back` | bool | whether using backward trajectory | +| `adapt_expansion_distance` | bool | if true, adapt expansion distance based on environment | | `expansion_distance` | double | length of expansion for node transitions | | `distance_heuristic_weight` | double | heuristic weight for estimating node's cost | | `smoothness_weight` | double | cost factor for change in curvature | +| `obstacle_distance_weight` | double | cost factor for distance to obstacle | #### RRT\* search parameters diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml index ee631f8639153..b4a1951514604 100644 --- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml +++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml @@ -32,9 +32,11 @@ astar: only_behind_solutions: false use_back: true + adapt_expansion_distance: true expansion_distance: 0.5 - distance_heuristic_weight: 1.0 + distance_heuristic_weight: 1.5 smoothness_weight: 0.5 + obstacle_distance_weight: 1.5 # -- RRT* search Configurations -- rrtstar: diff --git a/planning/autoware_freespace_planner/schema/freespace_planner.schema.json b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json new file mode 100644 index 0000000000000..9d17a07ee9204 --- /dev/null +++ b/planning/autoware_freespace_planner/schema/freespace_planner.schema.json @@ -0,0 +1,232 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for freespace_planner", + "type": "object", + "definitions": { + "freespace_planner_params": { + "type": "object", + "properties": { + "planning_algorithm": { + "type": "string", + "enum": ["astar", "rrtstar"], + "default": "astar", + "description": "Planning algorithm to use, options: astar, rrtstar." + }, + "waypoints_velocity": { + "type": "number", + "default": 5.0, + "description": "velocity in output trajectory (currently, only constant velocity is supported." + }, + "update_rate": { + "type": "number", + "default": 10.0, + "description": "timer's update rate" + }, + "th_arrived_distance_m": { + "type": "number", + "default": 1.0, + "description": "Threshold for considering the vehicle has arrived at its goal." + }, + "th_stopped_time_sec": { + "type": "number", + "default": 1.0, + "description": "Time threshold for considering the vehicle as stopped." + }, + "th_stopped_velocity_mps": { + "type": "number", + "default": 0.01, + "description": "Velocity threshold for considering the vehicle as stopped." + }, + "th_course_out_distance_m": { + "type": "number", + "default": 1.0, + "description": "Threshold distance for considering the vehicle has deviated from its course." + }, + "vehicle_shape_margin_m": { + "type": "number", + "default": 1.0, + "description": "Margin around the vehicle shape for planning." + }, + "replan_when_obstacle_found": { + "type": "boolean", + "default": true, + "description": "Replan path when an obstacle is found." + }, + "replan_when_course_out": { + "type": "boolean", + "default": true, + "description": "Replan path when the vehicle deviates from its course." + }, + "time_limit": { + "type": "number", + "default": 30000.0, + "description": "Time limit for the planner." + }, + "max_turning_ratio": { + "type": "number", + "default": 0.5, + "description": "Maximum turning ratio, relative to the actual turning limit of the vehicle." + }, + "turning_steps": { + "type": "number", + "default": 1, + "description": "Number of steps for turning." + }, + "theta_size": { + "type": "number", + "default": 144, + "description": "Number of discretized angles for search." + }, + "angle_goal_range": { + "type": "number", + "default": 6.0, + "description": "Range of acceptable angles at the goal." + }, + "lateral_goal_range": { + "type": "number", + "default": 0.5, + "description": "Lateral range of acceptable goal positions." + }, + "longitudinal_goal_range": { + "type": "number", + "default": 2.0, + "description": "Longitudinal range of acceptable goal positions." + }, + "curve_weight": { + "type": "number", + "default": 0.5, + "description": "Weight for curves in the cost function." + }, + "reverse_weight": { + "type": "number", + "default": 1.0, + "description": "Weight for reverse movement in the cost function." + }, + "direction_change_weight": { + "type": "number", + "default": 1.5, + "description": "Weight for direction changes in the cost function." + }, + "obstacle_threshold": { + "type": "number", + "default": 100, + "description": "Threshold for considering an obstacle in the costmap." + }, + "astar": { + "type": "object", + "properties": { + "only_behind_solutions": { + "type": "boolean", + "default": false, + "description": "Consider only solutions behind the vehicle." + }, + "use_back": { + "type": "boolean", + "default": true, + "description": "Allow reverse motion in A* search." + }, + "expansion_distance": { + "type": "number", + "default": 0.5, + "description": "Distance for expanding nodes in A* search." + }, + "distance_heuristic_weight": { + "type": "number", + "default": 1.0, + "description": "Weight for the distance heuristic in A* search." + }, + "smoothness_weight": { + "type": "number", + "default": 0.5, + "description": "Weight for the smoothness heuristic in A* search." + } + }, + "required": [ + "only_behind_solutions", + "use_back", + "expansion_distance", + "distance_heuristic_weight", + "smoothness_weight" + ] + }, + "rrtstar": { + "type": "object", + "properties": { + "enable_update": { + "type": "boolean", + "default": true, + "description": "Enable updates in RRT*." + }, + "use_informed_sampling": { + "type": "boolean", + "default": true, + "description": "Use informed sampling in RRT*." + }, + "max_planning_time": { + "type": "number", + "default": 150.0, + "description": "Maximum planning time for RRT*." + }, + "neighbor_radius": { + "type": "number", + "default": 8.0, + "description": "Radius for neighboring nodes in RRT*." + }, + "margin": { + "type": "number", + "default": 0.1, + "description": "Margin for RRT* sampling." + } + }, + "required": [ + "enable_update", + "use_informed_sampling", + "max_planning_time", + "neighbor_radius", + "margin" + ] + } + }, + "required": [ + "planning_algorithm", + "waypoints_velocity", + "update_rate", + "th_arrived_distance_m", + "th_stopped_time_sec", + "th_stopped_velocity_mps", + "th_course_out_distance_m", + "vehicle_shape_margin_m", + "replan_when_obstacle_found", + "replan_when_course_out", + "time_limit", + "max_turning_ratio", + "turning_steps", + "theta_size", + "angle_goal_range", + "lateral_goal_range", + "longitudinal_goal_range", + "curve_weight", + "reverse_weight", + "direction_change_weight", + "obstacle_threshold", + "astar", + "rrtstar" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/freespace_planner_params" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index d79a06ae52ead..b077f79f88071 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -573,6 +573,7 @@ void FreespacePlannerNode::initializePlanningAlgorithm() extended_vehicle_shape.length += margin; extended_vehicle_shape.width += margin; extended_vehicle_shape.base2back += margin / 2; + extended_vehicle_shape.setMinMaxDimension(); const auto planner_common_param = getPlannerCommonParam(); diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp index 4f0a59f44a5d6..cbf835df3b6c6 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp @@ -24,6 +24,7 @@ #include #include +#include #include namespace autoware::freespace_planning_algorithms @@ -63,6 +64,8 @@ struct VehicleShape double base_length; double max_steering; double base2back; // base_link to rear [m] + double min_dimension; + double max_dimension; VehicleShape() = default; @@ -74,6 +77,7 @@ struct VehicleShape max_steering(max_steering), base2back(base2back) { + setMinMaxDimension(); } explicit VehicleShape( @@ -84,6 +88,19 @@ struct VehicleShape max_steering(vehicle_info.max_steer_angle_rad), base2back(vehicle_info.rear_overhang_m + margin / 2.0) { + setMinMaxDimension(); + } + + void setMinMaxDimension() + { + const auto base2front = length - base2back; + if (base2back <= base2front) { + min_dimension = std::min(0.5 * width, base2back); + max_dimension = std::hypot(base2front, 0.5 * width); + } else { + min_dimension = std::min(0.5 * width, base2front); + max_dimension = std::hypot(base2back, 0.5 * width); + } } }; @@ -156,6 +173,16 @@ class AbstractPlanningAlgorithm std::vector & vertex_indexes_2d) const; bool detectBoundaryExit(const IndexXYT & base_index) const; bool detectCollision(const IndexXYT & base_index) const; + bool detectCollision(const geometry_msgs::msg::Pose & base_pose) const; + + // cspell: ignore Toriwaki + /// @brief Computes the euclidean distance to the nearest obstacle for each grid cell. + /// @cite T., Saito, and J., Toriwaki "New algorithms for euclidean distance transformation of an + /// n-dimensional digitized picture with applications," Pattern Recognition 27, 1994 + /// https://doi.org/10.1016/0031-3203(94)90133-3 + /// @details first, distance values are computed along each row. Then, the computed values are + /// used to to compute the minimum distance along each column. + void computeEDTMap(); template inline bool isOutOfRange(const IndexType & index) const @@ -194,6 +221,12 @@ class AbstractPlanningAlgorithm return is_obstacle_table_[indexToId(index)]; } + template + inline double getObstacleEDT(const IndexType & index) const + { + return edt_map_[indexToId(index)]; + } + // compute single dimensional grid cell index from 2 dimensional index template inline int indexToId(const IndexType & index) const @@ -216,6 +249,9 @@ class AbstractPlanningAlgorithm // is_obstacle's table std::vector is_obstacle_table_; + // Euclidean distance transform map (distance to nearest obstacle cell) + std::vector edt_map_; + // pose in costmap frame geometry_msgs::msg::Pose start_pose_; geometry_msgs::msg::Pose goal_pose_; diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp index f7434d912d5e0..cbdbb9d4c3fb3 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp @@ -43,11 +43,13 @@ struct AstarParam // base configs bool only_behind_solutions; // solutions should be behind the goal bool use_back; // backward search + bool adapt_expansion_distance; double expansion_distance; // search configs double distance_heuristic_weight; // obstacle threshold on grid [0,255] double smoothness_weight; + double obstacle_distance_weight; }; struct AstarNode @@ -56,9 +58,11 @@ struct AstarNode double x; // x double y; // y double theta; // theta - double gc = 0; // actual motion cost - double fc = 0; // total node cost - double dir_distance = 0; // distance traveled from last direction change + double gc = 0.0; // actual motion cost + double fc = 0.0; // total node cost + double dir_distance = 0.0; // distance traveled from last direction change + double dist_to_goal = 0.0; // euclidean distance to goal pose + double dist_to_obs = 0.0; // euclidean distance to nearest obstacle int steering_index; // steering index bool is_back; // true if the current direction of the vehicle is back AstarNode * parent = nullptr; // parent node @@ -82,46 +86,9 @@ struct NodeComparison bool operator()(const AstarNode * lhs, const AstarNode * rhs) const { return lhs->fc > rhs->fc; } }; -struct NodeUpdate -{ - double shift_x; - double shift_y; - double shift_theta; - double distance; - int steering_index; - bool is_back; - - NodeUpdate rotated(const double theta) const - { - NodeUpdate result = *this; - result.shift_x = std::cos(theta) * this->shift_x - std::sin(theta) * this->shift_y; - result.shift_y = std::sin(theta) * this->shift_x + std::cos(theta) * this->shift_y; - return result; - } - - NodeUpdate flipped() const - { - NodeUpdate result = *this; - result.shift_y = -result.shift_y; - result.shift_theta = -result.shift_theta; - return result; - } - - NodeUpdate reversed() const - { - NodeUpdate result = *this; - result.shift_x = -result.shift_x; - result.shift_theta = -result.shift_theta; - result.is_back = !result.is_back; - return result; - } -}; - class AstarSearch : public AbstractPlanningAlgorithm { public: - using TransitionTable = std::vector>; - AstarSearch( const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, const AstarParam & astar_param); @@ -134,12 +101,15 @@ class AstarSearch : public AbstractPlanningAlgorithm AstarParam{ node.declare_parameter("astar.only_behind_solutions"), node.declare_parameter("astar.use_back"), + node.declare_parameter("astar.adapt_expansion_distance"), node.declare_parameter("astar.expansion_distance"), node.declare_parameter("astar.distance_heuristic_weight"), - node.declare_parameter("astar.smoothness_weight")}) + node.declare_parameter("astar.smoothness_weight"), + node.declare_parameter("astar.obstacle_distance_weight")}) { } + void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override; bool makePlan(const Pose & start_pose, const Pose & goal_pose) override; const PlannerWaypoints & getWaypoints() const { return waypoints_; } @@ -150,27 +120,26 @@ class AstarSearch : public AbstractPlanningAlgorithm } private: - void setTransitionTable(); void setCollisionFreeDistanceMap(); bool search(); - void expandNodes(AstarNode & current_node); + void expandNodes(AstarNode & current_node, const bool is_back = false); void resetData(); void setPath(const AstarNode & goal); bool setStartNode(); - bool setGoalNode(); double estimateCost(const Pose & pose, const IndexXYT & index) const; bool isGoal(const AstarNode & node) const; Pose node2pose(const AstarNode & node) const; + double getExpansionDistance(const AstarNode & current_node) const; double getSteeringCost(const int steering_index) const; double getSteeringChangeCost(const int steering_index, const int prev_steering_index) const; double getDirectionChangeCost(const double dir_distance) const; + double getObsDistanceCost(const double obs_distance) const; // Algorithm specific param AstarParam astar_param_; // hybrid astar variables - TransitionTable transition_table_; std::vector graph_; std::vector col_free_distance_map_; @@ -185,6 +154,20 @@ class AstarSearch : public AbstractPlanningAlgorithm double steering_resolution_; double heading_resolution_; double avg_turning_radius_; + double min_expansion_dist_; + double max_expansion_dist_; + + // the following constexpr values were found to be best by trial and error, through multiple + // tests, and are not expected to be changed regularly, therefore they were not made into ros + // parameters. + + // expansion distance factors + static constexpr double base_length_max_expansion_factor_ = 0.5; + static constexpr double dist_to_goal_expansion_factor_ = 0.15; + static constexpr double dist_to_obs_expansion_factor_ = 0.3; + + // cost free obstacle distance + static constexpr double cost_free_obs_dist = 5.0; }; } // namespace autoware::freespace_planning_algorithms diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp index 0a59646f34d52..b9b6fe8c79af0 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp @@ -31,39 +31,26 @@ static constexpr double eps = 0.001; inline double getTurningRadius(const double base_length, const double steering_angle) { - return base_length / tan(steering_angle); -} - -inline geometry_msgs::msg::Pose getPoseShift( - const double yaw, const double base_length, const double steering_angle, const double distance) -{ - geometry_msgs::msg::Pose pose; - if (abs(steering_angle) < eps) { - pose.position.x = distance * cos(yaw); - pose.position.y = distance * sin(yaw); - return pose; - } - const double R = getTurningRadius(base_length, steering_angle); - const double beta = distance / R; - pose.position.x = R * sin(yaw + beta) - R * sin(yaw); - pose.position.y = R * cos(yaw) - R * cos(yaw + beta); - pose.orientation = autoware::universe_utils::createQuaternionFromYaw(beta); - return pose; + return base_length / std::tan(steering_angle); } inline geometry_msgs::msg::Pose getPose( const geometry_msgs::msg::Pose & current_pose, const double base_length, const double steering_angle, const double distance) { - const double current_yaw = tf2::getYaw(current_pose.orientation); - const auto shift = getPoseShift(current_yaw, base_length, steering_angle, distance); auto pose = current_pose; - pose.position.x += shift.position.x; - pose.position.y += shift.position.y; - if (abs(steering_angle) > eps) { - pose.orientation = autoware::universe_utils::createQuaternionFromYaw( - current_yaw + tf2::getYaw(shift.orientation)); + const double yaw = tf2::getYaw(current_pose.orientation); + if (std::abs(steering_angle) < eps) { + pose.position.x += distance * std::cos(yaw); + pose.position.y += distance * std::sin(yaw); + return pose; } + + const double R = getTurningRadius(base_length, steering_angle); + const double beta = distance / R; + pose.position.x += (R * std::sin(yaw + beta) - R * std::sin(yaw)); + pose.position.y += (R * std::cos(yaw) - R * std::cos(yaw + beta)); + pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw + beta); return pose; } diff --git a/planning/autoware_freespace_planning_algorithms/package.xml b/planning/autoware_freespace_planning_algorithms/package.xml index 05e906c998096..ecd8cfb7adb02 100644 --- a/planning/autoware_freespace_planning_algorithms/package.xml +++ b/planning/autoware_freespace_planning_algorithms/package.xml @@ -23,10 +23,13 @@ nlohmann-json-dev pybind11_vendor rosbag2_cpp + rosbag2_storage std_msgs tf2 tf2_geometry_msgs + rosbag2_storage_mcap + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index 70de72f2bfffc..40b16f71d5fe7 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -114,13 +114,19 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) .def_readwrite( "only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions) .def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back) + .def_readwrite( + "adapt_expansion_distance", + &freespace_planning_algorithms::AstarParam::adapt_expansion_distance) .def_readwrite( "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance) .def_readwrite( "distance_heuristic_weight", &freespace_planning_algorithms::AstarParam::distance_heuristic_weight) .def_readwrite( - "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight); + "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight) + .def_readwrite( + "obstacle_distance_weight", + &freespace_planning_algorithms::AstarParam::obstacle_distance_weight); auto pyPlannerCommonParam = py::class_( p, "PlannerCommonParam", py::dynamic_attr()) diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py index 93e3acbc345b6..28c505fbaae02 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -46,9 +46,11 @@ astar_param = fp.AstarParam() astar_param.only_behind_solutions = False astar_param.use_back = True +astar_param.adapt_expansion_distance = True astar_param.expansion_distance = 0.4 astar_param.distance_heuristic_weight = 1.0 astar_param.smoothness_weight = 1.0 +astar_param.obstacle_distance_weight = 1.0 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param) diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index 7ec594d389de3..380eafb745062 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -120,6 +120,8 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost } is_obstacle_table_ = is_obstacle_table; + computeEDTMap(); + // construct collision indexes table if (is_collision_table_initialized == false) { for (int i = 0; i < planner_common_param_.theta_size; i++) { @@ -136,6 +138,73 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost std::hypot(0.5 * collision_vehicle_shape_.width, base2front) / costmap_.info.resolution); } +void AbstractPlanningAlgorithm::computeEDTMap() +{ + const int height = costmap_.info.height; + const int width = costmap_.info.width; + const double resolution_m = costmap_.info.resolution; + const double diagonal_m = std::hypot(resolution_m * height, resolution_m * width); + std::vector edt_map; + edt_map.reserve(costmap_.data.size()); + + std::vector temporary_storage(width); + // scan rows + for (int i = 0; i < height; ++i) { + double distance = resolution_m; + bool found_obstacle = false; + // forward scan + for (int j = 0; j < width; ++j) { + if (isObs(IndexXY{j, i})) { + temporary_storage[j] = 0.0; + distance = resolution_m; + found_obstacle = true; + } else if (found_obstacle) { + temporary_storage[j] = distance; + distance += resolution_m; + } else { + temporary_storage[j] = diagonal_m; + } + } + + distance = resolution_m; + found_obstacle = false; + // backward scan + for (int j = width - 1; j >= 0; --j) { + if (isObs(IndexXY{j, i})) { + distance = resolution_m; + found_obstacle = true; + } else if (found_obstacle && temporary_storage[j] > distance) { + temporary_storage[j] = distance; + distance += resolution_m; + } + } + edt_map.insert(edt_map.end(), temporary_storage.begin(), temporary_storage.end()); + } + + temporary_storage.clear(); + temporary_storage.resize(height); + // scan columns; + for (int j = 0; j < width; ++j) { + for (int i = 0; i < height; ++i) { + int id = indexToId(IndexXY{j, i}); + double min_value = edt_map[id] * edt_map[id]; + for (int k = 0; k < height; ++k) { + id = indexToId(IndexXY{j, k}); + double dist = resolution_m * std::abs(static_cast(i - k)); + double value = edt_map[id] * edt_map[id] + dist * dist; + if (value < min_value) { + min_value = value; + } + } + temporary_storage[i] = sqrt(min_value); + } + for (int i = 0; i < height; ++i) { + edt_map[indexToId(IndexXY{j, i})] = temporary_storage[i]; + } + } + edt_map_ = edt_map; +} + void AbstractPlanningAlgorithm::computeCollisionIndexes( int theta_index, std::vector & indexes_2d, std::vector & vertex_indexes_2d) const @@ -218,6 +287,12 @@ bool AbstractPlanningAlgorithm::detectBoundaryExit(const IndexXYT & base_index) return false; } +bool AbstractPlanningAlgorithm::detectCollision(const geometry_msgs::msg::Pose & base_pose) const +{ + const auto base_index = pose2index(costmap_, base_pose, planner_common_param_.theta_size); + return detectCollision(base_index); +} + bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const { if (coll_indexes_table_.empty()) { @@ -227,6 +302,13 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con if (detectBoundaryExit(base_index)) return true; + double obstacle_edt = getObstacleEDT(base_index); + + // if nearest obstacle is further than largest dimension, no collision is guaranteed + // if nearest obstacle is closer than smallest dimension, collision is guaranteed + if (obstacle_edt > collision_vehicle_shape_.max_dimension) return false; + if (obstacle_edt < collision_vehicle_shape_.min_dimension) return true; + const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta]; for (const auto & coll_index_2d : coll_indexes_2d) { IndexXY coll_index{coll_index_2d.x, coll_index_2d.y}; @@ -247,9 +329,7 @@ bool AbstractPlanningAlgorithm::hasObstacleOnTrajectory( { for (const auto & pose : trajectory.poses) { const auto pose_local = global2local(costmap_, pose); - const auto index = pose2index(costmap_, pose_local, planner_common_param_.theta_size); - - if (detectCollision(index)) { + if (detectCollision(pose_local)) { return true; } } diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 9233ad212bfa5..fe87011273c1b 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -33,6 +33,8 @@ namespace autoware::freespace_planning_algorithms { +using autoware::universe_utils::calcDistance2d; + double calcReedsSheppDistance(const Pose & p1, const Pose & p2, double radius) { const auto rs_space = ReedsSheppStateSpace(radius); @@ -81,37 +83,18 @@ AstarSearch::AstarSearch( avg_turning_radius_ = kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering); - setTransitionTable(); + min_expansion_dist_ = astar_param_.expansion_distance; + max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_; } -void AstarSearch::setTransitionTable() +void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) { - const double distance = astar_param_.expansion_distance; - transition_table_.resize(planner_common_param_.theta_size); - - std::vector forward_transitions; - int steering_ind = -1 * planner_common_param_.turning_steps; - for (; steering_ind <= planner_common_param_.turning_steps; ++steering_ind) { - const double steering = static_cast(steering_ind) * steering_resolution_; - Pose shift_pose = kinematic_bicycle_model::getPoseShift( - 0.0, collision_vehicle_shape_.base_length, steering, distance); - forward_transitions.push_back( - {shift_pose.position.x, shift_pose.position.y, tf2::getYaw(shift_pose.orientation), distance, - steering_ind, false}); - } - - for (int i = 0; i < planner_common_param_.theta_size; ++i) { - const double theta = static_cast(i) * heading_resolution_; - for (const auto & transition : forward_transitions) { - transition_table_[i].push_back(transition.rotated(theta)); - } + AbstractPlanningAlgorithm::setMap(costmap); - if (astar_param_.use_back) { - for (const auto & transition : forward_transitions) { - transition_table_[i].push_back(transition.reversed().rotated(theta)); - } - } - } + // ensure minimum expansion distance is larger then grid cell diagonal length + min_expansion_dist_ = std::max(astar_param_.expansion_distance, 1.5 * costmap_.info.resolution); + max_expansion_dist_ = std::max( + collision_vehicle_shape_.base_length * base_length_max_expansion_factor_, min_expansion_dist_); } bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) @@ -121,7 +104,7 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) start_pose_ = global2local(costmap_, start_pose); goal_pose_ = global2local(costmap_, goal_pose); - if (!setGoalNode()) { + if (detectCollision(goal_pose_)) { throw std::logic_error("Invalid goal pose"); } @@ -180,6 +163,7 @@ void AstarSearch::setCollisionFreeDistanceMap() const IndexXY n_index{x, y}; const double offset = std::abs(offset_x) + std::abs(offset_y); if (isOutOfRange(n_index) || isObs(n_index) || offset < 1) continue; + if (getObstacleEDT(n_index) < 0.5 * collision_vehicle_shape_.width) continue; const int n_id = indexToId(n_index); const double dist = current.second + (sqrt(offset) * costmap_.info.resolution); if (closed[n_id] || col_free_distance_map_[n_id] < dist) continue; @@ -200,6 +184,8 @@ bool AstarSearch::setStartNode() AstarNode * start_node = &graph_[getKey(index)]; start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false); start_node->dir_distance = 0.0; + start_node->dist_to_goal = calcDistance2d(start_pose_, goal_pose_); + start_node->dist_to_obs = getObstacleEDT(index); start_node->status = NodeStatus::Open; start_node->parent = nullptr; @@ -209,12 +195,6 @@ bool AstarSearch::setStartNode() return true; } -bool AstarSearch::setGoalNode() -{ - const auto index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size); - return !detectCollision(index); -} - double AstarSearch::estimateCost(const Pose & pose, const IndexXYT & index) const { double total_cost = col_free_distance_map_[indexToId(index)]; @@ -252,28 +232,29 @@ bool AstarSearch::search() } expandNodes(*current_node); + if (astar_param_.use_back) expandNodes(*current_node, true); } // Failed to find path return false; } -void AstarSearch::expandNodes(AstarNode & current_node) +void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back) { - const auto index_theta = discretizeAngle(current_node.theta, planner_common_param_.theta_size); - for (const auto & transition : transition_table_[index_theta]) { - // skip transition back to parent + const auto current_pose = node2pose(current_node); + double distance = getExpansionDistance(current_node) * (is_back ? -1.0 : 1.0); + int steering_index = -1 * planner_common_param_.turning_steps; + for (; steering_index <= planner_common_param_.turning_steps; ++steering_index) { + // skip expansion back to parent if ( - current_node.parent != nullptr && transition.is_back != current_node.is_back && - transition.steering_index == current_node.steering_index) { + current_node.parent != nullptr && is_back != current_node.is_back && + steering_index == current_node.steering_index) { continue; } - // Calculate index of the next state - Pose next_pose; - next_pose.position.x = current_node.x + transition.shift_x; - next_pose.position.y = current_node.y + transition.shift_y; - setYaw(&next_pose.orientation, current_node.theta + transition.shift_theta); + const double steering = static_cast(steering_index) * steering_resolution_; + const auto next_pose = kinematic_bicycle_model::getPose( + current_pose, collision_vehicle_shape_.base_length, steering, distance); const auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size); if (isOutOfRange(next_index) || isObs(next_index)) continue; @@ -281,27 +262,28 @@ void AstarSearch::expandNodes(AstarNode & current_node) AstarNode * next_node = &graph_[getKey(next_index)]; if (next_node->status == NodeStatus::Closed || detectCollision(next_index)) continue; + const double distance_to_obs = getObstacleEDT(next_index); const bool is_direction_switch = - (current_node.parent != nullptr) && (transition.is_back != current_node.is_back); + (current_node.parent != nullptr) && (is_back != current_node.is_back); double total_weight = 1.0; - total_weight += getSteeringCost(transition.steering_index); - if (transition.is_back) { - total_weight *= (1.0 + planner_common_param_.reverse_weight); - } + total_weight += getSteeringCost(steering_index); + if (is_back) total_weight *= (1.0 + planner_common_param_.reverse_weight); - double move_cost = current_node.gc + (total_weight * transition.distance); - move_cost += getSteeringChangeCost(transition.steering_index, current_node.steering_index); + double move_cost = current_node.gc + (total_weight * std::abs(distance)); + move_cost += getSteeringChangeCost(steering_index, current_node.steering_index); + move_cost += getObsDistanceCost(distance_to_obs); if (is_direction_switch) move_cost += getDirectionChangeCost(current_node.dir_distance); double total_cost = move_cost + estimateCost(next_pose, next_index); // Compare cost if (next_node->status == NodeStatus::None || next_node->fc > total_cost) { next_node->status = NodeStatus::Open; - next_node->set( - next_pose, move_cost, total_cost, transition.steering_index, transition.is_back); + next_node->set(next_pose, move_cost, total_cost, steering_index, is_back); next_node->dir_distance = - transition.distance + (is_direction_switch ? 0.0 : current_node.dir_distance); + std::abs(distance) + (is_direction_switch ? 0.0 : current_node.dir_distance); + next_node->dist_to_goal = calcDistance2d(next_pose, goal_pose_); + next_node->dist_to_obs = distance_to_obs; next_node->parent = ¤t_node; openlist_.push(next_node); continue; @@ -309,6 +291,17 @@ void AstarSearch::expandNodes(AstarNode & current_node) } } +double AstarSearch::getExpansionDistance(const AstarNode & current_node) const +{ + if (!astar_param_.adapt_expansion_distance || max_expansion_dist_ <= min_expansion_dist_) { + return min_expansion_dist_; + } + double exp_dist = std::min( + current_node.dist_to_goal * dist_to_goal_expansion_factor_, + current_node.dist_to_obs * dist_to_obs_expansion_factor_); + return std::clamp(exp_dist, min_expansion_dist_, max_expansion_dist_); +} + double AstarSearch::getSteeringCost(const int steering_index) const { return planner_common_param_.curve_weight * @@ -328,6 +321,12 @@ double AstarSearch::getDirectionChangeCost(const double dir_distance) const return planner_common_param_.direction_change_weight * (1.0 + (1.0 / (1.0 + dir_distance))); } +double AstarSearch::getObsDistanceCost(const double obs_distance) const +{ + return astar_param_.obstacle_distance_weight * + std::max(1.0 - (obs_distance / cost_free_obs_dist), 0.0); +} + void AstarSearch::setPath(const AstarNode & goal_node) { std_msgs::msg::Header header; @@ -342,10 +341,27 @@ void AstarSearch::setPath(const AstarNode & goal_node) geometry_msgs::msg::PoseStamped pose; pose.header = header; + + const auto interpolate = [this, &pose](const AstarNode & node) { + if (node.parent == nullptr || !astar_param_.adapt_expansion_distance) return; + const auto parent_pose = node2pose(*node.parent); + const double distance_2d = calcDistance2d(node2pose(node), parent_pose); + const int n = static_cast(distance_2d / min_expansion_dist_); + for (int i = 1; i < n; ++i) { + const double dist = ((distance_2d * i) / n) * (node.is_back ? -1.0 : 1.0); + const double steering = node.steering_index * steering_resolution_; + const auto local_pose = kinematic_bicycle_model::getPose( + parent_pose, collision_vehicle_shape_.base_length, steering, dist); + pose.pose = local2global(costmap_, local_pose); + waypoints_.waypoints.push_back({pose, node.is_back}); + } + }; + // push astar nodes poses while (node != nullptr) { pose.pose = local2global(costmap_, node2pose(*node)); waypoints_.waypoints.push_back({pose, node->is_back}); + interpolate(*node); // To the next node node = node->parent; } diff --git a/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp index 97149dbbb071f..ee6d1cb8d1d9a 100644 --- a/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp @@ -676,7 +676,7 @@ ReedsSheppStateSpace::StateXYT ReedsSheppStateSpace::interpolate( } StateXYT s_out; - double phi, v; + double v; s_out.x = s_out.y = 0.0; s_out.yaw = s0.yaw; @@ -689,7 +689,7 @@ ReedsSheppStateSpace::StateXYT ReedsSheppStateSpace::interpolate( v = std::min(seg, path.length_[i]); seg -= v; } - phi = s_out.yaw; + double phi = s_out.yaw; switch (path.type_[i]) { case RS_LEFT: s_out.x += (sin(phi + v) - std::sin(phi)); diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 0ef31959e65fa..20a59048e59d1 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -113,9 +113,7 @@ bool RRTStar::hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & traj { for (const auto & pose : trajectory.poses) { const auto pose_local = global2local(costmap_, pose); - const auto base_index = pose2index(costmap_, pose_local, planner_common_param_.theta_size); - - if (detectCollision(base_index)) { + if (detectCollision(pose_local)) { return true; } } diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 31c551e1906db..7284a8dcffc44 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -211,12 +211,14 @@ std::unique_ptr configure_astar(bool use_multi) // configure astar param const bool only_behind_solutions = false; const bool use_back = true; + const bool adapt_expansion_distance = true; const double expansion_distance = 0.4; const double distance_heuristic_weight = 2.0; const double smoothness_weight = 0.5; + const double obstacle_distance_weight = 1.7; const auto astar_param = fpa::AstarParam{ - only_behind_solutions, use_back, expansion_distance, distance_heuristic_weight, - smoothness_weight}; + only_behind_solutions, use_back, adapt_expansion_distance, expansion_distance, + distance_heuristic_weight, smoothness_weight, obstacle_distance_weight}; auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); return algo; diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp index 19e28ee2abfc7..5394caf698664 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -44,6 +44,8 @@ struct DefaultPlannerParameters class DefaultPlanner : public mission_planner::PlannerPlugin { public: + DefaultPlanner() : is_graph_ready_(false), route_handler_(), param_(), node_(nullptr) {} + void initialize(rclcpp::Node * node) override; void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override; bool ready() const override; diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 6473c3150dcab..2480c4bd02753 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -16,6 +16,7 @@ rosidl_default_generators autoware_behavior_path_planner_common + autoware_geography_utils autoware_lanelet2_extension autoware_map_msgs autoware_mission_planner @@ -27,7 +28,6 @@ autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils - geography_utils geometry_msgs global_parameter_loader interpolation diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 4b9c5a73e81bc..e9958b74059a5 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -30,9 +30,9 @@ #include "type_alias.hpp" #include "utils.hpp" +#include #include #include -#include #include #include "std_msgs/msg/empty.hpp" @@ -871,7 +871,8 @@ void StaticCenterlineGeneratorNode::save_map() // save map with modified center line std::filesystem::create_directory("/tmp/autoware_static_centerline_generator"); - const auto map_projector = geography_utils::get_lanelet2_projector(*map_projector_info_); + const auto map_projector = + autoware::geography_utils::get_lanelet2_projector(*map_projector_info_); lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); RCLCPP_INFO( get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 014aaf6db6657..2cfefeb741e80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -205,15 +205,16 @@ The main thread will be the one called from the planner manager flow. - The path candidates generated there are referred to by the main thread, and the one judged to be valid for the current planner data (e.g. ego and object information) is selected from among them. valid means no sudden deceleration, no collision with obstacles, etc. The selected path will be the output of this module. - If there is no path selected, or if the selected path is collision and ego is stuck, a separate thread(freespace path generation thread) will generate a path using freespace planning algorithm. If a valid free space path is found, it will be the output of the module. If the object moves and the pull over path generated along the lane is collision-free, the path is used as output again. See also the section on freespace parking for more information on the flow of generating freespace paths. -| Name | Unit | Type | Description | Default value | -| :------------------------------- | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | -| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | -| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path | -| efficient_path_order | [-] | string | efficient order of pull over planner along lanes excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path | +| efficient_path_order | [-] | string | efficient order of pull over planner along lanes excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | +| lane_departure_check_expansion_margin | [m] | double | margin to expand the ego vehicle footprint when doing lane departure checks | 0.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 61908b524c253..0d8d9c29304f8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -54,6 +54,7 @@ maximum_jerk: 1.0 path_priority: "efficient_path" # "efficient_path" or "close_goal" efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking) + lane_departure_check_expansion_margin: 0.0 # shift parking shift_parking: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 749ed57f95cc9..aaad9d5f1a8f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -143,38 +143,21 @@ class ThreadSafeData void set_pull_over_path(const PullOverPath & path) { const std::lock_guard lock(mutex_); - pull_over_path_ = std::make_shared(path); - if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = std::make_shared(path); - } - - last_path_update_time_ = clock_->now(); + set_pull_over_path_no_lock(path); } void set_pull_over_path(const std::shared_ptr & path) { const std::lock_guard lock(mutex_); - pull_over_path_ = path; - if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = path; - } - last_path_update_time_ = clock_->now(); + set_pull_over_path_no_lock(path); } template void set(Args... args) { std::lock_guard lock(mutex_); - (..., set(args)); + (..., set_no_lock(args)); } - void set(const GoalCandidates & arg) { set_goal_candidates(arg); } - void set(const std::vector & arg) { set_pull_over_path_candidates(arg); } - void set(const std::shared_ptr & arg) { set_pull_over_path(arg); } - void set(const PullOverPath & arg) { set_pull_over_path(arg); } - void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); } - void set(const BehaviorModuleOutput & arg) { set_last_previous_module_output(arg); } - void set(const PreviousPullOverData & arg) { set_prev_data(arg); } - void set(const CollisionCheckDebugMap & arg) { set_collision_check(arg); } void clearPullOverPath() { @@ -232,6 +215,34 @@ class ThreadSafeData DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) private: + void set_pull_over_path_no_lock(const PullOverPath & path) + { + pull_over_path_ = std::make_shared(path); + if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = std::make_shared(path); + } + + last_path_update_time_ = clock_->now(); + } + + void set_pull_over_path_no_lock(const std::shared_ptr & path) + { + pull_over_path_ = path; + if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = path; + } + last_path_update_time_ = clock_->now(); + } + + void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } + void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } + void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } + void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } + void set_no_lock(const PreviousPullOverData & arg) { prev_data_ = arg; } + void set_no_lock(const CollisionCheckDebugMap & arg) { collision_check_ = arg; } + std::shared_ptr pull_over_path_{nullptr}; std::shared_ptr lane_parking_pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 6a10c0fb183f6..1272ea38451ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -89,6 +89,7 @@ struct GoalPlannerParameters double maximum_jerk{0.0}; std::string path_priority; // "efficient_path" or "close_goal" std::vector efficient_path_order{}; + double lane_departure_check_expansion_margin{0.0}; // shift path bool enable_shift_parking{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 419f3bd2dda47..6dee8d7b701bc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -68,6 +68,10 @@ GoalPlannerModule::GoalPlannerModule( { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); + lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters->lane_departure_check_expansion_margin; + lane_departure_checker.setParam(lane_departure_checker_params); occupancy_grid_map_ = std::make_shared(); @@ -1953,6 +1957,8 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr bool is_driving_forward = true; constexpr bool is_pull_out = false; + constexpr bool is_lane_change = false; + constexpr bool is_pull_over = true; const bool override_ego_stopped_check = std::invoke([&]() { if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) { return false; @@ -1968,7 +1974,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, is_driving_forward, - egos_lane_is_shifted, override_ego_stopped_check, is_pull_out); + egos_lane_is_shifted, override_ego_stopped_check, is_pull_out, is_lane_change, is_pull_over); ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); return new_signal; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index a9cd754edcd7f..805e91f739a6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -137,6 +137,8 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.path_priority = node->declare_parameter(ns + "path_priority"); p.efficient_path_order = node->declare_parameter>(ns + "efficient_path_order"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_margin"); } // shift parking @@ -518,6 +520,9 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); updateParam( parameters, ns + "after_shift_straight_distance", p->after_shift_straight_distance); + updateParam( + parameters, ns + "lane_departure_check_expansion_margin", + p->lane_departure_check_expansion_margin); } // parallel parking common diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index cc695f820ee38..ff3af92442fbe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -53,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase void update_lanes(const bool is_approved) final; + void update_filtered_objects() final; + void updateLaneChangeStatus() override; std::pair getSafePath(LaneChangePath & safe_path) const override; @@ -92,6 +94,8 @@ class NormalLaneChange : public LaneChangeBase bool isAbleToReturnCurrentLane() const override; + bool is_near_terminal() const final; + bool isEgoOnPreparePhase() const override; bool isAbleToStopSafely() const override; @@ -155,9 +159,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, LaneChangePaths * candidate_paths, - const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, - const bool check_safety = true) const override; + Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index ccc9258324ffa..6c7dc31e857e5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -67,6 +67,8 @@ class LaneChangeBase virtual void update_lanes(const bool is_approved) = 0; + virtual void update_filtered_objects() = 0; + virtual void updateLaneChangeStatus() = 0; virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; @@ -93,6 +95,8 @@ class LaneChangeBase virtual bool isAbleToReturnCurrentLane() const = 0; + virtual bool is_near_terminal() const = 0; + virtual LaneChangePath getLaneChangePath() const = 0; virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; @@ -234,12 +238,6 @@ class LaneChangeBase const lanelet::ConstLanelets & current_lanes, const double backward_path_length, const double prepare_length) const = 0; - virtual bool getLaneChangePaths( - const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, - const bool is_stuck, const bool check_safety) const = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; @@ -256,6 +254,7 @@ class LaneChangeBase std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; lane_change::CommonDataPtr common_data_ptr_{}; + FilteredByLanesExtendedObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 421b54db9f67a..6c48445b47567 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -19,6 +19,7 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation { using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::LCParamPtr; /** * @brief Calculates the distance from the ego vehicle to the terminal point. @@ -40,6 +41,40 @@ double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr); double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, const Pose & src_pose); + +/** + * @brief Calculates the minimum stopping distance to terminal start. + * + * This function computes the minimum stopping distance to terminal start based on the + * minimum lane changing velocity and the minimum longitudinal acceleration. It then + * compares this calculated distance with a pre-defined backward length buffer parameter + * and returns the larger of the two values to ensure safe lane changing. + * + * @param lc_param_ptr Shared pointer to an LCParam structure, which should include: + * - `minimum_lane_changing_velocity`: The minimum velocity required for lane changing. + * - `min_longitudinal_acc`: The minimum longitudinal acceleration used for deceleration. + * - `backward_length_buffer_for_end_of_lane`: A predefined backward buffer length parameter. + * + * @return The required backward buffer distance in meters. + */ +double calc_stopping_distance(const LCParamPtr & lc_param_ptr); + +/** + * @brief Calculates the distance to last fit width position along the lane + * + * This function computes the distance from the current ego position to the last + * position along the lane where the ego foot prints stays within the lane + * boundaries. + * + * @param lanelets current ego lanelets + * @param src_pose source pose to calculate distance from + * @param bpp_param common parameters used in behavior path planner. + * @param margin additional margin for checking lane width + * @return distance to last fit width position along the lane + */ +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index afbfc67caa2f7..a7cf89193b37f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -76,6 +76,7 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); + module_type_->update_filtered_objects(); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -265,6 +266,15 @@ bool LaneChangeInterface::canTransitFailureState() return true; } + if (module_type_->is_near_terminal()) { + log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change"); + + if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { + log_debug_throttled("Module require stopping"); + } + return false; + } + if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { if (module_type_->isStoppedAtRedTrafficLight()) { log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 11e1c20e57adb..23925d56badb3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -99,6 +99,11 @@ void NormalLaneChange::update_lanes(const bool is_approved) *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); } +void NormalLaneChange::update_filtered_objects() +{ + filtered_objects_ = filterObjects(); +} + void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -125,15 +130,9 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) LaneChangePaths valid_paths{}; const bool is_stuck = isVehicleStuck(current_lanes); - bool found_safe_path = getLaneChangePaths( - current_lanes, target_lanes, direction_, &valid_paths, lane_change_parameters_->rss_params, - is_stuck); + bool found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction_, is_stuck, &valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin - if (!found_safe_path && is_stuck) { - found_safe_path = getLaneChangePaths( - current_lanes, target_lanes, direction_, &valid_paths, - lane_change_parameters_->rss_params_for_stuck, is_stuck); - } lane_change_debug_.valid_paths = valid_paths; @@ -301,6 +300,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!status_.is_valid_path) { RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); + insertStopPoint(get_current_lanes(), prev_module_output_.path); return prev_module_output_; } @@ -397,6 +397,14 @@ void NormalLaneChange::insertStopPoint( const auto target_objects = filterObjects(); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { + const double distance_to_last_fit_width = + utils::lane_change::calculation::calc_dist_to_last_fit_width( + lanelets, path.points.front().point.pose, planner_data_->parameters); + stopping_distance = std::min(stopping_distance, distance_to_last_fit_width); + } + const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start; if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { @@ -603,10 +611,12 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's // current lane, lane change is different, so we set this flag to false. constexpr bool egos_lane_is_shifted = false; + constexpr bool is_pull_out = false; + constexpr bool is_lane_change = true; const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward, - egos_lane_is_shifted); + egos_lane_is_shifted, is_pull_out, is_lane_change); return new_signal; } @@ -735,8 +745,9 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); if (!utils::isEgoWithinOriginalLane( - get_current_lanes(), getEgoPose(), planner_data_->parameters, + curr_lanes_poly, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; @@ -757,7 +768,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( - get_current_lanes(), estimated_pose, planner_data_->parameters, + curr_lanes_poly, estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; return is_ego_within_original_lane; @@ -768,6 +779,31 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return true; } +bool NormalLaneChange::is_near_terminal() const +{ + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + + if (current_lanes.empty()) { + return true; + } + + const auto & current_lanes_terminal = current_lanes.back(); + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + const auto direction = common_data_ptr_->direction; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + const auto min_lane_changing_distance = calcMinimumLaneChangeLength( + route_handler_ptr, current_lanes_terminal, *lc_param_ptr, direction); + + const auto backward_buffer = calculation::calc_stopping_distance(lc_param_ptr); + + const auto min_lc_dist_with_buffer = + backward_buffer + min_lane_changing_distance + lc_param_ptr->lane_change_finish_judge_buffer; + const auto dist_from_ego_to_terminal_end = + calculation::calc_ego_dist_to_terminal_end(common_data_ptr_); + + return dist_from_ego_to_terminal_end < min_lc_dist_with_buffer; +} + bool NormalLaneChange::isEgoOnPreparePhase() const { const auto & start_position = status_.lane_change_path.info.shift_line.start.position; @@ -790,13 +826,14 @@ bool NormalLaneChange::isAbleToStopSafely() const const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - get_current_lanes(), estimated_pose, planner_data_->parameters); + curr_lanes_poly, estimated_pose, planner_data_->parameters); } } return true; @@ -1162,7 +1199,7 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( }; if ( - check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && + check_optional_polygon(object, lanes_polygon.target) && is_lateral_far && is_before_terminal()) { const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); @@ -1174,6 +1211,20 @@ FilteredByLanesObjects NormalLaneChange::filterObjectsByLanelets( continue; } + if ( + check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && + is_before_terminal()) { + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, object); + constexpr double stopped_obj_vel_th = 1.0; + if (object.kinematics.initial_twist_with_covariance.twist.linear.x < stopped_obj_vel_th) { + if (ahead_of_ego) { + target_lane_leading_objects.push_back(object); + continue; + } + } + } + const auto is_overlap_target_backward = std::invoke([&]() -> bool { const auto check_backward_polygon = [&object](const auto & target_backward_polygon) { return isPolygonOverlapLanelet(object, target_backward_polygon); @@ -1329,9 +1380,7 @@ bool NormalLaneChange::hasEnoughLengthToTrafficLight( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, LaneChangePaths * candidate_paths, - const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, - const bool check_safety) const + Direction direction, const bool is_stuck, LaneChangePaths * candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); if (current_lanes.empty() || target_lanes.empty()) { @@ -1378,8 +1427,7 @@ bool NormalLaneChange::getLaneChangePaths( return false; } - const auto filtered_objects = filterObjects(); - const auto target_objects = getTargetObjects(filtered_objects, current_lanes); + const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1627,7 +1675,7 @@ bool NormalLaneChange::getLaneChangePaths( if ( !is_stuck && !utils::lane_change::passed_parked_objects( - common_data_ptr_, *candidate_path, filtered_objects.target_lane_leading, + common_data_ptr_, *candidate_path, filtered_objects_.target_lane_leading, lane_change_buffer, lane_change_debug_.collision_check_objects)) { debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " @@ -1635,13 +1683,20 @@ bool NormalLaneChange::getLaneChangePaths( return false; } - if (!check_safety) { - debug_print_lat("ACCEPT!!!: it is valid (and safety check is skipped)."); - return false; - } + const auto is_safe = std::invoke([&]() { + const auto safety_check_with_normal_rss = isLaneChangePathSafe( + *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, + lane_change_debug_.collision_check_objects); + + if (!safety_check_with_normal_rss.is_safe && is_stuck) { + const auto safety_check_with_stuck_rss = isLaneChangePathSafe( + *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck, + lane_change_debug_.collision_check_objects); + return safety_check_with_stuck_rss.is_safe; + } - const auto [is_safe, is_trailing_object] = isLaneChangePathSafe( - *candidate_path, target_objects, rss_params, lane_change_debug_.collision_check_objects); + return safety_check_with_normal_rss.is_safe; + }); if (is_safe) { debug_print_lat("ACCEPT!!!: it is valid and safe!"); @@ -1808,8 +1863,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {true, true}; } - const auto filtered_objects = filterObjects(); - const auto target_objects = getTargetObjects(filtered_objects, current_lanes); + const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); CollisionCheckDebugMap debug_data; @@ -1818,7 +1872,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( - common_data_ptr_, path, filtered_objects.target_lane_leading, min_lc_length, debug_data); + common_data_ptr_, path, filtered_objects_.target_lane_leading, min_lc_length, debug_data); if (!has_passed_parked_objects) { RCLCPP_DEBUG(logger_, "Lane change has been delayed."); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 521c30d406e7a..8f04395be572a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -1,3 +1,4 @@ + // Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -15,6 +16,8 @@ #include #include +#include + namespace autoware::behavior_path_planner::utils::lane_change::calculation { double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) @@ -42,4 +45,60 @@ double calc_dist_from_pose_to_terminal_end( } return utils::getDistanceToEndOfLane(src_pose, lanes); } + +double calc_stopping_distance(const LCParamPtr & lc_param_ptr) +{ + // v^2 = u^2 + 2ad + const auto min_lc_vel = lc_param_ptr->minimum_lane_changing_velocity; + const auto min_lon_acc = lc_param_ptr->min_longitudinal_acc; + const auto min_back_dist = std::abs((min_lc_vel * min_lc_vel) / (2 * min_lon_acc)); + + const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; + return std::max(min_back_dist, param_back_dist); +} + +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; + universe_utils::MultiPolygon2d center_line_polygon; + namespace strategy = boost::geometry::strategy::buffer; + boost::geometry::buffer( + line_string, center_line_polygon, strategy::distance_symmetric(buffer_distance), + strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), + strategy::point_square()); + + if (center_line_polygon.empty()) return 0.0; + + std::vector intersection_points; + boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + + if (intersection_points.empty()) { + return utils::getDistanceToEndOfLane(src_pose, lanelets); + } + + Pose pose; + double distance = std::numeric_limits::max(); + for (const auto & point : intersection_points) { + pose.position.x = boost::geometry::get<0>(point); + pose.position.y = boost::geometry::get<1>(point); + distance = std::min(distance, utils::getSignedDistance(src_pose, pose, lanelets)); + } + + return std::max(distance - (bpp_param.base_link2front + margin), 0.0); +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_develop_maps.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_develop_maps.md new file mode 100644 index 0000000000000..1c3c01857e73f --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_develop_maps.md @@ -0,0 +1,11 @@ +# Develop purpose maps + +## road_shoulders + +The road_shoulders lanelet map consist of a variety of pick-up/drop-off site maps with road_shoulder tags including: + +- pick-up/drop-off sites on the side of street lanes +- pick-up/drop-off sites on the side of curved lanes +- pick-up/drop-off sites inside a private area + +![road_shoulder_test](./road_shoulder_test_map.png) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/road_shoulder_test_map.png b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/road_shoulder_test_map.png new file mode 100644 index 0000000000000..0815bd93538a5 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/road_shoulder_test_map.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 1b0bb240cabf2..12fb872ded9ac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp @@ -174,7 +174,8 @@ struct PlannerData const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx, const lanelet::ConstLanelets & current_lanelets, const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, - const bool override_ego_stopped_check = false, const bool is_pull_out = false) const + const bool override_ego_stopped_check = false, const bool is_pull_out = false, + const bool is_lane_change = false, const bool is_pull_over = false) const { if (shift_start_idx + 1 > path.points.size()) { RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); @@ -211,7 +212,7 @@ struct PlannerData return turn_signal_decider.getBehaviorTurnSignalInfo( shifted_path, shift_line, current_lanelets, route_handler, parameters, self_odometry, current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check, - is_pull_out); + is_pull_out, is_lane_change, is_pull_over); } std::pair getBehaviorTurnSignalInfo( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp index bc41c77a07cea..90cf533a8589d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp @@ -134,7 +134,8 @@ class TurnSignalDecider const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check = false, - const bool is_pull_out = false) const; + const bool is_pull_out = false, const bool is_lane_change = false, + const bool is_pull_over = false) const; private: std::optional getIntersectionTurnSignalInfo( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 05f7c70fa42c2..f70e64730d119 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -59,9 +59,10 @@ Polygon2d createExtendedPolygon( const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug); + Polygon2d createExtendedPolygon( - const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - const bool is_stopped_obj, CollisionCheckDebug & debug); + const PoseWithVelocityAndPolygonStamped & obj_pose_with_poly, const double lon_length, + const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug); PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 5c1eee92c2a5d..03e6c2d7f8167 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -55,6 +55,8 @@ using geometry_msgs::msg::Vector3; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; +static constexpr double eps = 0.01; + struct PolygonPoint { geometry_msgs::msg::Point point; @@ -241,6 +243,10 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); + // path management // TODO(Horibe) There is a similar function in route_handler. Check. @@ -249,8 +255,10 @@ std::shared_ptr generateCenterLinePath( PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, const bool left_side); double getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, const double base_link2rear, const Pose & vehicle_pose, const bool left_side); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index 2a9d3fb4d1d13..0e005166a97c0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -652,7 +652,8 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const std::shared_ptr route_handler, const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, - const bool override_ego_stopped_check, const bool is_pull_out) const + const bool override_ego_stopped_check, const bool is_pull_out, const bool is_lane_change, + const bool is_pull_over) const { using autoware::universe_utils::getPose; @@ -770,15 +771,18 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); if ( - !is_pull_out && !existShiftSideLane( - start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, - p.turn_signal_shift_length_threshold)) { + (!is_pull_out && !is_lane_change && !is_pull_over) && + !existShiftSideLane( + start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, + p.turn_signal_shift_length_threshold)) { return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // Check if the ego will cross lane bounds. // Note that pull out requires blinkers, even if the ego does not cross lane bounds - if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { + if ( + (!is_pull_out && !is_pull_over) && + !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 8d2dd2fb86e29..1726638c1bf9b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -129,6 +129,7 @@ Polygon2d createExtendedPolygon( autoware::universe_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; + polygon.outer().reserve(5); appendPointToPolygon(polygon, p1.position); appendPointToPolygon(polygon, p2.position); appendPointToPolygon(polygon, p3.position); @@ -140,13 +141,14 @@ Polygon2d createExtendedPolygon( } Polygon2d createExtendedPolygon( - const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - const bool is_stopped_obj, CollisionCheckDebug & debug) + const PoseWithVelocityAndPolygonStamped & obj_pose_with_poly, const double lon_length, + const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, shape); + const auto & obj_polygon = obj_pose_with_poly.poly; if (obj_polygon.outer().empty()) { return obj_polygon; } + const auto & obj_pose = obj_pose_with_poly.pose; double max_x = std::numeric_limits::lowest(); double min_x = std::numeric_limits::max(); @@ -186,6 +188,7 @@ Polygon2d createExtendedPolygon( autoware::universe_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; + polygon.outer().reserve(5); appendPointToPolygon(polygon, p1.position); appendPointToPolygon(polygon, p2.position); appendPointToPolygon(polygon, p3.position); @@ -670,10 +673,9 @@ std::vector getCollidedPolygons( throw std::domain_error("invalid rss parameter. please select 'rectangle' or 'along_path'."); }(); const auto & extended_obj_polygon = - is_object_front - ? obj_polygon - : createExtendedPolygon( - obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug); + is_object_front ? obj_polygon + : createExtendedPolygon( + obj_pose_with_poly, lon_offset, lat_margin, is_stopped_object, debug); // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index aa16f1bb23b79..0f3751cc32dd3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -559,19 +559,24 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin) { - const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); - const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); - const auto base_link2front = common_param.base_link2front; - const auto base_link2rear = common_param.base_link2rear; - const auto vehicle_width = common_param.vehicle_width; + const auto combined_lane = lanelet::utils::combineLaneletsShape(current_lanes); + const auto lane_polygon = combined_lane.polygon2d().basicPolygon(); + return isEgoWithinOriginalLane(lane_polygon, current_pose, common_param, outer_margin); +} + +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin) +{ const auto vehicle_poly = autoware::universe_utils::toFootprint( - current_pose, base_link2front, base_link2rear, vehicle_width); + current_pose, common_param.base_link2front, common_param.base_link2rear, + common_param.vehicle_width); // Check if the ego vehicle is entirely within the lane with a given outer margin. for (const auto & p : vehicle_poly.outer()) { // When the point is in the polygon, the distance is 0. When it is out of the polygon, return a // positive value. - const auto dist = boost::geometry::distance(p, lanelet::utils::to2D(lane_poly).basicPolygon()); + const auto dist = boost::geometry::distance(p, lane_polygon); if (dist > std::max(outer_margin, 0.0)) { return false; // out of polygon } @@ -816,25 +821,29 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) return path.points.at(*insert_idx); } +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, bool left_side) +{ + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(position); + const auto & boundary_line_2d = left_side ? lanelet.leftBound2d() : lanelet.rightBound2d(); + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + return arc_coordinates.distance; +} + double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose, bool left_side) { lanelet::ConstLanelet closest_lanelet; - lanelet::ArcCoordinates arc_coordinates; + if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); - const auto & boundary_line_2d = left_side - ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - arc_coordinates = lanelet::geometry::toArcCoordinates( - boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "closest shoulder lanelet not found."); + return getSignedDistanceFromLaneBoundary(closest_lanelet, pose.position, left_side); } - return arc_coordinates.distance; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), "closest lanelet not found."); + + return 0.0; } std::optional getSignedDistanceFromBoundary( @@ -1211,7 +1220,6 @@ PathWithLaneId setDecelerationVelocity( const auto stop_point_length = autoware::motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; - constexpr double eps{0.01}; if (std::abs(target_velocity) < eps && stop_point_length > 0.0) { const auto stop_point = utils::insertStopPoint(stop_point_length, reference_path); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 85a80eac8f215..2708b1e608779 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -161,8 +161,14 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) const bool is_stopped_object = false; CollisionCheckDebug debug; + + using autoware::behavior_path_planner::utils::path_safety_checker:: + PoseWithVelocityAndPolygonStamped; + + PoseWithVelocityAndPolygonStamped obj_pose_with_poly( + 0.0, obj_pose, 0.0, autoware::universe_utils::toPolygon2d(obj_pose, shape)); const auto polygon = - createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, is_stopped_object, debug); + createExtendedPolygon(obj_pose_with_poly, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_map/road_shoulder.osm b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_map/road_shoulder.osm new file mode 100644 index 0000000000000..88f1da37f46d1 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test_map/road_shoulder.osm @@ -0,0 +1,9984 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 846781359ebce..76e617adb0fe1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -772,6 +772,46 @@ bool isObviousAvoidanceTarget( } } + if (object.behavior == ObjectData::Behavior::MERGING) { + object.info = ObjectInfo::MERGING_TO_EGO_LANE; + if ( + isOnRight(object) && !object.is_on_ego_lane && + object.overhang_points.front().first < parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "merging vehicle. but overhang distance is less than threshold."); + return true; + } + if ( + !isOnRight(object) && !object.is_on_ego_lane && + object.overhang_points.front().first > -1.0 * parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "merging vehicle. but overhang distance is less than threshold."); + return true; + } + } + + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.info = ObjectInfo::DEVIATING_FROM_EGO_LANE; + if ( + isOnRight(object) && !object.is_on_ego_lane && + object.overhang_points.front().first < parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "deviating vehicle. but overhang distance is less than threshold."); + return true; + } + if ( + !isOnRight(object) && !object.is_on_ego_lane && + object.overhang_points.front().first > -1.0 * parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "deviating vehicle. but overhang distance is less than threshold."); + return true; + } + } + if (!object.is_parked) { object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md index 318758220c239..825bb4043d993 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md @@ -647,3 +647,15 @@ This module is activated when the following conditions are met: ### Known Issue If ego go over the stop line for a certain distance, then it will not transit from STOP. + +## Test Maps + +The intersections lanelet map consist of a variety of intersections including: + +- 4-way crossing with traffic light +- 4-way crossing without traffic light +- T-shape crossing without traffic light +- intersection with a loop +- complicated intersection + +![intersection_test](./docs/intersection_test_map.png) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index 7772235342266..39ed187cd867a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -10,6 +10,7 @@ Tomoya Kimura Shumpei Wakabayashi Kyoichi Sugahara + Yukinari Hisaki Apache License 2.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt index 9cb992312f52a..49ab46e26bd17 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -26,4 +26,4 @@ if(BUILD_TESTING) ) endif() -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE test_map) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md index 2abbb83575af5..311728b6b58d5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md @@ -1,3 +1,17 @@ # Behavior Velocity Planner Common This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules. + +## Test map + +### intersection + +The intersections lanelet map consist of a variety of intersections including: + +- 4-way crossing with traffic light +- 4-way crossing without traffic light +- T-shape crossing without traffic light +- intersection with a loop +- complicated intersection + +![intersection_test](./docs/intersection_test_map.png) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/docs/intersection_test_map.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/docs/intersection_test_map.png new file mode 100644 index 0000000000000..a2f3db6ad2a31 Binary files /dev/null and b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/docs/intersection_test_map.png differ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test_map/intersection/pointcloud_map.pcd b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test_map/intersection/pointcloud_map.pcd new file mode 100644 index 0000000000000..d97cbe19e0085 Binary files /dev/null and b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test_map/intersection/pointcloud_map.pcd differ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp index 86ec8335e6362..30ed9ae65a8a0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp @@ -70,6 +70,7 @@ struct ObstacleParameters node.get_logger(), static_cast(node.declare_parameter(RTREE_SEGMENTS_PARAM))); } + // cppcheck-suppress functionStatic bool updateType(const rclcpp::Logger & logger, const std::string & type) { if (type == "pointcloud") { @@ -140,6 +141,7 @@ struct ProjectionParameters duration = node.declare_parameter(DURATION_PARAM); } + // cppcheck-suppress functionStatic bool updateModel(const rclcpp::Logger & logger, const std::string & model_str) { if (model_str == "particle") { @@ -154,6 +156,7 @@ struct ProjectionParameters return true; } + // cppcheck-suppress functionStatic bool updateDistanceMethod(const rclcpp::Logger & logger, const std::string & method_str) { if (method_str == "exact") { diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index a7a48180bf64a..868dd96088a65 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -217,13 +217,12 @@ void DistortionCorrector::undistortPointCloud( // If there is a point in a pointcloud that cannot be associated, record it to issue a warning bool is_twist_time_stamp_too_late = false; bool is_imu_time_stamp_too_late = false; - double global_point_stamp; for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { bool is_twist_valid = true; bool is_imu_valid = true; - global_point_stamp = + const double global_point_stamp = pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp); // Get closest twist information diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index eef6d690b6e72..e43013cfc37ec 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -19,12 +19,12 @@ libboost-dev + autoware_geography_utils autoware_sensing_msgs component_interface_specs component_interface_utils geographic_msgs geographiclib - geography_utils geometry_msgs rclcpp rclcpp_components diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 40a60d17ea7db..4cd3661edeb50 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -14,8 +14,8 @@ #include "gnss_poser/gnss_poser_core.hpp" -#include -#include +#include +#include #include @@ -116,8 +116,9 @@ void GNSSPoser::callback_nav_sat_fix( gps_point.latitude = nav_sat_fix_msg_ptr->latitude; gps_point.longitude = nav_sat_fix_msg_ptr->longitude; gps_point.altitude = nav_sat_fix_msg_ptr->altitude; - geometry_msgs::msg::Point position = geography_utils::project_forward(gps_point, projector_info_); - position.z = geography_utils::convert_height( + geometry_msgs::msg::Point position = + autoware::geography_utils::project_forward(gps_point, projector_info_); + position.z = autoware::geography_utils::convert_height( position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84, projector_info_.vertical_datum); diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 95ced648f1326..c2751c96e995c 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,6 +15,7 @@ autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs + autoware_geography_utils autoware_motion_utils autoware_planning_msgs autoware_system_msgs @@ -24,7 +25,6 @@ component_interface_utils diagnostic_graph_utils geographic_msgs - geography_utils nav_msgs rclcpp rclcpp_components diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index 47932f969c172..053e1258c7bef 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,8 +14,8 @@ #include "vehicle.hpp" -#include -#include +#include +#include #include @@ -143,15 +143,17 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.twist.header.frame_id = kinematic_state_msgs_->child_frame_id; vehicle_kinematics.twist.twist = kinematic_state_msgs_->twist; if (map_projector_info_->projector_type != MapProjectorInfo::LOCAL) { - const geographic_msgs::msg::GeoPoint projected_gps_point = geography_utils::project_reverse( - kinematic_state_msgs_->pose.pose.position, *map_projector_info_); + const geographic_msgs::msg::GeoPoint projected_gps_point = + autoware::geography_utils::project_reverse( + kinematic_state_msgs_->pose.pose.position, *map_projector_info_); vehicle_kinematics.geographic_pose.header = kinematic_state_msgs_->header; vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.latitude; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.longitude; - vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( - projected_gps_point.altitude, projected_gps_point.latitude, projected_gps_point.longitude, - map_projector_info_->vertical_datum, MapProjectorInfo::WGS84); + vehicle_kinematics.geographic_pose.position.altitude = + autoware::geography_utils::convert_height( + projected_gps_point.altitude, projected_gps_point.latitude, projected_gps_point.longitude, + map_projector_info_->vertical_datum, MapProjectorInfo::WGS84); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = diff --git a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp b/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp index b5e0e51c0ceaf..13d7151bb14c7 100644 --- a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp +++ b/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp @@ -175,14 +175,6 @@ void CPUMonitorBase::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & st return; } - std::string cpu_name; - float usr{0.0}; - float nice{0.0}; - float sys{0.0}; - float iowait{0.0}; - float idle{0.0}; - float usage{0.0}; - float total{0.0}; int level = DiagStatus::OK; int whole_level = DiagStatus::OK; @@ -203,6 +195,15 @@ void CPUMonitorBase::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & st CpuStatus cpu_status; + std::string cpu_name; + float usr{0.0}; + float nice{0.0}; + float sys{0.0}; + float iowait{0.0}; + float idle{0.0}; + float usage{0.0}; + float total{0.0}; + if (boost::optional v = cpu_load.get_optional("cpu")) { cpu_name = v.get(); get_cpu_name = true; diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 2ab3e053225eb..73059d2d162fa 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -127,8 +127,6 @@ void ProcessMonitor::getTasksSummary( } bp::pipe p{p_fd[0], p_fd[1]}; - std::string line; - // Echo output for grep { int out_fd[2]; @@ -179,6 +177,7 @@ void ProcessMonitor::getTasksSummary( return; } + std::string line; std::getline(is_out, line); std::cmatch match; const std::regex filter(