diff --git a/common/autoware_auto_common/design/comparisons.md b/common/autoware_auto_common/design/comparisons.md index f3a7eb5e58429..3d00f77c1b0c3 100644 --- a/common/autoware_auto_common/design/comparisons.md +++ b/common/autoware_auto_common/design/comparisons.md @@ -22,8 +22,8 @@ The `exclusive_or` function will test whether two values cast to different boole ## Example Usage ```c++ -#include "common/bool_comparisons.hpp" -#include "common/float_comparisons.hpp" +#include "autoware_auto_common/common/bool_comparisons.hpp" +#include "autoware_auto_common/common/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/include/common/type_traits.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp similarity index 96% rename from common/autoware_auto_common/include/common/type_traits.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp index 7087ed1e81181..66f382f081b33 100644 --- a/common/autoware_auto_common/include/common/type_traits.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp @@ -14,15 +14,15 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" #include #include #include -#ifndef COMMON__TYPE_TRAITS_HPP_ -#define COMMON__TYPE_TRAITS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ namespace autoware { @@ -219,4 +219,4 @@ struct intersect } // namespace common } // namespace autoware -#endif // COMMON__TYPE_TRAITS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp similarity index 93% rename from common/autoware_auto_common/include/common/types.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/types.hpp index 3f3e444c1aa8c..1c7dfe48c7ec8 100644 --- a/common/autoware_auto_common/include/common/types.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp @@ -16,11 +16,11 @@ /// \file /// \brief This file includes common type definition -#ifndef COMMON__TYPES_HPP_ -#define COMMON__TYPES_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ -#include "common/visibility_control.hpp" -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include #include @@ -122,4 +122,4 @@ using void_t = void; } // namespace common } // namespace autoware -#endif // COMMON__TYPES_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ diff --git a/common/autoware_auto_common/include/common/visibility_control.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp similarity index 88% rename from common/autoware_auto_common/include/common/visibility_control.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp index 0a988d6407dfa..33834fd9ccfed 100644 --- a/common/autoware_auto_common/include/common/visibility_control.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef COMMON__VISIBILITY_CONTROL_HPP_ -#define COMMON__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ #if defined(_MSC_VER) && defined(_WIN64) #if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) @@ -35,4 +35,4 @@ #error "Unsupported Build Configuration" #endif // _MSC_VER -#endif // COMMON__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp similarity index 89% rename from common/autoware_auto_common/include/helper_functions/angle_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp index 6cca2440d5680..ea974774dd9d5 100644 --- a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ -#define HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ #include #include @@ -63,4 +63,4 @@ constexpr T wrap_angle(T angle) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp similarity index 85% rename from common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp index c6bf09365af4f..45da098ad0066 100644 --- a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp @@ -18,10 +18,10 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#include "common/types.hpp" +#include "autoware_auto_common/common/types.hpp" namespace autoware { @@ -47,4 +47,4 @@ types::bool8_t exclusive_or(const T & a, const T & b) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp similarity index 90% rename from common/autoware_auto_common/include/helper_functions/byte_reader.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp index d9e55c4ecfbfe..3852361caebeb 100644 --- a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__BYTE_READER_HPP_ -#define HELPER_FUNCTIONS__BYTE_READER_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ #include #include @@ -70,4 +70,4 @@ class ByteReader } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BYTE_READER_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/crtp.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp similarity index 88% rename from common/autoware_auto_common/include/helper_functions/crtp.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp index 0e8110a9a3bb9..e75674eb73c70 100644 --- a/common/autoware_auto_common/include/helper_functions/crtp.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__CRTP_HPP_ -#define HELPER_FUNCTIONS__CRTP_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ namespace autoware { @@ -49,4 +49,4 @@ class crtp } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__CRTP_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp similarity index 95% rename from common/autoware_auto_common/include/helper_functions/float_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp index de1f459f21d0a..1a64fe71a1720 100644 --- a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp @@ -18,8 +18,8 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ #include #include @@ -146,4 +146,4 @@ bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp similarity index 92% rename from common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp index fb9bdccf32b25..70c29eaf220d8 100644 --- a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ -#define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ #include @@ -69,4 +69,4 @@ types::float32_t calculate_mahalanobis_distance( } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp similarity index 94% rename from common/autoware_auto_common/include/helper_functions/message_adapters.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index 352ef7c7b65d5..d3bda57b3374f 100644 --- a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ -#define HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ #include @@ -112,4 +112,4 @@ TimeStamp get_stamp(const T & msg) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/template_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp similarity index 91% rename from common/autoware_auto_common/include/helper_functions/template_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp index b39931a3fd15a..0360908509618 100644 --- a/common/autoware_auto_common/include/helper_functions/template_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#define HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#include +#include "autoware_auto_common/common/types.hpp" #include @@ -72,4 +72,4 @@ struct expression_valid_with_return< } // namespace helper_functions } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/type_name.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp similarity index 84% rename from common/autoware_auto_common/include/helper_functions/type_name.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp index a95834373d552..106cede1f2f5a 100644 --- a/common/autoware_auto_common/include/helper_functions/type_name.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#define HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#include +#include "autoware_auto_common/common/visibility_control.hpp" #include #include @@ -53,4 +53,4 @@ COMMON_PUBLIC std::string get_type_name(const T &) } // namespace helper_functions } // namespace autoware -#endif // HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ diff --git a/common/autoware_auto_common/test/test_angle_utils.cpp b/common/autoware_auto_common/test/test_angle_utils.cpp index 031ae144139aa..810c302845daf 100644 --- a/common/autoware_auto_common/test/test_angle_utils.cpp +++ b/common/autoware_auto_common/test/test_angle_utils.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/angle_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_bool_comparisons.cpp b/common/autoware_auto_common/test/test_bool_comparisons.cpp index 67c1c8ddbf9aa..a84d65e569692 100644 --- a/common/autoware_auto_common/test/test_bool_comparisons.cpp +++ b/common/autoware_auto_common/test/test_bool_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/bool_comparisons.hpp" +#include "autoware_auto_common/helper_functions/bool_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_byte_reader.cpp b/common/autoware_auto_common/test/test_byte_reader.cpp index c83d06c6e8132..a5ab8743f7ed4 100644 --- a/common/autoware_auto_common/test/test_byte_reader.cpp +++ b/common/autoware_auto_common/test/test_byte_reader.cpp @@ -14,9 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "common/types.hpp" -#include "gtest/gtest.h" -#include "helper_functions/byte_reader.hpp" +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/byte_reader.hpp" + +#include #include diff --git a/common/autoware_auto_common/test/test_float_comparisons.cpp b/common/autoware_auto_common/test/test_float_comparisons.cpp index d292dc0a0cb20..37d3afdc177d5 100644 --- a/common/autoware_auto_common/test/test_float_comparisons.cpp +++ b/common/autoware_auto_common/test/test_float_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp index 262599180abb6..2015a85bc2bc8 100644 --- a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp +++ b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp @@ -13,8 +13,8 @@ // limitations under the License. // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/mahalanobis_distance.hpp" #include diff --git a/common/autoware_auto_common/test/test_message_field_adapters.cpp b/common/autoware_auto_common/test/test_message_field_adapters.cpp index 34974c1cda9a6..c35f0ff826995 100644 --- a/common/autoware_auto_common/test/test_message_field_adapters.cpp +++ b/common/autoware_auto_common/test/test_message_field_adapters.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/message_adapters.hpp" #include diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp index 9c9ca9ae4b5f2..a670aaab83cfa 100644 --- a/common/autoware_auto_common/test/test_template_utils.cpp +++ b/common/autoware_auto_common/test/test_template_utils.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/template_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_name.cpp b/common/autoware_auto_common/test/test_type_name.cpp index 1fb60d838f307..4ada59b4fb2e1 100644 --- a/common/autoware_auto_common/test/test_type_name.cpp +++ b/common/autoware_auto_common/test/test_type_name.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/type_name.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_traits.cpp b/common/autoware_auto_common/test/test_type_traits.cpp index 7203ab8d649ee..92d01b3d84d51 100644 --- a/common/autoware_auto_common/test/test_type_traits.cpp +++ b/common/autoware_auto_common/test/test_type_traits.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/type_traits.hpp" +#include "autoware_auto_common/common/types.hpp" #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp index da7121bf4e9b5..e3c2e58c9f80e 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp @@ -21,7 +21,7 @@ #include "autoware_auto_geometry/interval.hpp" -#include +#include #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp index e566e2e43ae3e..ae81c55ad6b55 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp @@ -23,7 +23,7 @@ #include "autoware_auto_geometry/common_2d.hpp" -#include +#include // lint -e537 NOLINT pclint vs cpplint #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp index fd04c0266e065..cd9fd49f71635 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp @@ -23,7 +23,7 @@ #include "autoware_auto_geometry/common_2d.hpp" -#include +#include #include #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp index 17e2789ac0325..54be2c32b1d05 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp @@ -21,8 +21,8 @@ #ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ #define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#include "common/types.hpp" -#include "helper_functions/float_comparisons.hpp" +#include +#include #include #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp index c689638aa2bd9..7b8867ca096ae 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp @@ -21,7 +21,8 @@ #define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ #include "autoware_auto_geometry/interval.hpp" -#include "common/types.hpp" + +#include #include #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp index c160685fb0f8c..8936e2c17d779 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp @@ -23,7 +23,7 @@ #include "autoware_auto_geometry/spatial_hash_config.hpp" #include "autoware_auto_geometry/visibility_control.hpp" -#include +#include #include #include diff --git a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp index 926b170f807c2..24c4d6e879d4a 100644 --- a/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp @@ -22,9 +22,9 @@ #include "autoware_auto_geometry/common_2d.hpp" #include "autoware_auto_geometry/visibility_control.hpp" -#include "helper_functions/crtp.hpp" -#include +#include +#include #include #include diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp index 092f8a04f2540..81865656c55b5 100644 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ b/common/autoware_auto_geometry/test/src/lookup_table.cpp @@ -16,7 +16,7 @@ #include "autoware_auto_geometry/lookup_table.hpp" -#include +#include #include diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 1fdc44df11183..b661010c19a4d 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -98,6 +99,11 @@ get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr( const std::string & uuid, const geometry_msgs::msg::Point & centroid, diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 683877f6429bd..223611520b2fc 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -14,12 +14,12 @@ #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "rviz_common/properties/enum_property.hpp" +#include "common/color_alpha_property.hpp" -#include #include #include #include +#include #include #include #include @@ -77,6 +77,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_display_path_confidence_property{ "Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization", this}, + + m_display_existence_probability_property{ + "Display Existence Probability", false, "Enable/disable existence probability visualization", + this}, + m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this}, m_default_topic{default_topic} { @@ -202,6 +207,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return std::nullopt; } } + template + std::optional get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const ClassificationContainerT & labels) const + { + if (m_display_existence_probability_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_existence_probability_marker_ptr( + centroid, orientation, existence_probability, color_rgba); + } else { + return std::nullopt; + } + } template std::optional get_uuid_marker_ptr( @@ -326,7 +344,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } return (it->second).label; } - std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const { std::stringstream ss; @@ -415,6 +432,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_predicted_paths_property; // Property to enable/disable predicted path confidence visualization rviz_common::properties::BoolProperty m_display_path_confidence_property; + + rviz_common::properties::BoolProperty m_display_existence_probability_property; + // Property to decide line width of object shape rviz_common::properties::FloatProperty m_line_width_property; // Default topic name to be visualized diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp index ac9c5af4ddeef..57a42c95f9d34 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 2ba80fddc1f34..5dcdce791c585 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -57,6 +57,22 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(label_marker_ptr); } + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y; + existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation, + existence_probability, object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = id++; + add_marker(existence_prob_marker_ptr); + } + // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = object.kinematics.pose_with_covariance.pose.position.x - 0.5; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index d10bc8ef5ab9d..d87541b7840a9 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -242,6 +242,23 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("existence probability"); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.z = 0.5; + marker_ptr->text = std::to_string(existence_probability); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->color = color_rgba; + return marker_ptr; +} + visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 5aca4290e5ac3..438d70f052b4a 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -121,6 +121,25 @@ std::vector PredictedObjectsDisplay: markers.push_back(pose_with_covariance_marker_ptr); } + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = + object.kinematics.initial_pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = + object.kinematics.initial_pose_with_covariance.pose.position.y; + existence_probability_position.z = + object.kinematics.initial_pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, + object.kinematics.initial_pose_with_covariance.pose.orientation, existence_probability, + object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(existence_prob_marker_ptr); + } // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = uuid_vis_position.x - 0.5; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 603e39f0c46b5..75b094b49a762 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -102,7 +102,21 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(pose_with_covariance_marker_ptr); } - + // Get marker for existence probability + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y; + existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation, + existence_probability, object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(existence_prob_marker_ptr); + } // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = uuid_vis_position.x - 0.5; diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index b35eb6e93ce6e..50c16ba8eaf7d 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -17,7 +17,7 @@ #ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ #define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#include +#include #include #include diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp index bc48e2a0294e0..97f46933f2fe6 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp @@ -52,7 +52,7 @@ struct NodeInterface {ServiceLog::CLIENT_RESPONSE, "client exit"}, {ServiceLog::ERROR_UNREADY, "client unready"}, {ServiceLog::ERROR_TIMEOUT, "client timeout"}}); - RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name); + RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name); ServiceLog msg; msg.stamp = node->now(); diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index d66d0fed3033c..e065f332b75e4 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,7 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include +#include #include #include diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp index 804ae47ff0f4e..6e77c1ec4a186 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -17,6 +17,7 @@ #include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "std_msgs/msg/header.hpp" #include @@ -34,7 +35,8 @@ namespace motion_utils * points larger than the capacity. (Tier IV) */ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory); + const std::vector & trajectory, + const std_msgs::msg::Header & header = std_msgs::msg::Header{}); /** * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 1566b493c82f8..834d07a87ea09 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -486,7 +486,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = resampled_path.right_bound; + resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp index 97a0bcd06e8cc..f198003d84091 100644 --- a/common/motion_utils/src/trajectory/conversion.cpp +++ b/common/motion_utils/src/trajectory/conversion.cpp @@ -30,9 +30,11 @@ namespace motion_utils * points larger than the capacity. (Tier IV) */ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory) + const std::vector & trajectory, + const std_msgs::msg::Header & header) { autoware_auto_planning_msgs::msg::Trajectory output{}; + output.header = header; for (const auto & pt : trajectory) output.points.push_back(pt); return output; } diff --git a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp index aedbe4b34ea22..16ff5a011536c 100644 --- a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp @@ -41,20 +41,20 @@ class Client const typename ServiceT::Request::SharedPtr & request, const std::chrono::nanoseconds & timeout = std::chrono::seconds(2)) { - RCLCPP_INFO(logger_, "client request"); + RCLCPP_DEBUG(logger_, "client request"); if (!client_->service_is_ready()) { - RCLCPP_INFO(logger_, "client available"); + RCLCPP_DEBUG(logger_, "client available"); return {response_error("Internal service is not available."), nullptr}; } auto future = client_->async_send_request(request); if (future.wait_for(timeout) != std::future_status::ready) { - RCLCPP_INFO(logger_, "client timeout"); + RCLCPP_DEBUG(logger_, "client timeout"); return {response_error("Internal service has timed out."), nullptr}; } - RCLCPP_INFO(logger_, "client response"); + RCLCPP_DEBUG(logger_, "client response"); return {response_success(), future.get()}; } diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index 9610bf1e2b7f1..fdd270fcce2ef 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -263,9 +263,9 @@ void ManualController::onClickEnableButton() { auto req = std::make_shared(); req->engage = true; - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client_engage_->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client_engage_->async_send_request( diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index eba3e4eacd275..8f67a90215bd1 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -171,15 +171,15 @@ public Q_SLOTS: // NOLINT for Qt { auto req = std::make_shared(); - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_INFO( + RCLCPP_DEBUG( raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str()); }); diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 081eab28e8833..6ce95acc4ef1d 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,10 +15,13 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#include "autoware_perception_msgs/msg/traffic_signal.hpp" +#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" #include "tier4_perception_msgs/msg/traffic_signal.hpp" +#include #include #include #include @@ -36,6 +39,47 @@ bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal); void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float confidence = -1); +/** + * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. + * + * Iterates through the traffic light elements to find a circle-shaped light that matches the given + * color. + * + * @param tl_state The traffic light state to check. + * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. + * @return True if a circle-shaped light with the specified color is found, false otherwise. + */ +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + +/** + * @brief Checks if a traffic light state includes a light with the specified shape. + * + * Searches through the traffic light elements to find a light that matches the given shape. + * + * @param tl_state The traffic light state to check. + * @param shape The shape to look for in the traffic light's lights. + * @return True if a light with the specified shape is found, false otherwise. + */ +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param tl_state The current state of the traffic light associated with the lanelet. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state); + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); tf2::Vector3 getTrafficLightBottomRight(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/package.xml b/common/traffic_light_utils/package.xml index de05355eafd66..37b4d46ce356a 100644 --- a/common/traffic_light_utils/package.xml +++ b/common/traffic_light_utils/package.xml @@ -6,6 +6,7 @@ The traffic_light_utils package Mingyu Li Shunsuke Miura + Satoshi Ota Apache License 2.0 ament_cmake_auto @@ -15,6 +16,7 @@ ament_lint_auto autoware_lint_common + autoware_perception_msgs lanelet2_extension tier4_perception_msgs diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index e02798bb41767..c8f2ca85b2089 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -51,6 +51,63 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float } } +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light) { const auto & tl_bl = traffic_light.front(); diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4518de37b7034..b8ee8ad79a33d 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -37,6 +37,7 @@ | `steering_angle_velocity` | double | steering angle velocity for operation | | `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | | `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `raw_control` | bool | skip input odometry if true | | `velocity_gain` | double | ratio to calculate velocity by acceleration | | `max_forward_velocity` | double | absolute max velocity to go forward | | `max_backward_velocity` | double | absolute max velocity to go backward | @@ -85,3 +86,25 @@ | Autoware Disengage | ○ | | Vehicle Engage | △ | | Vehicle Disengage | △ | + +## XBOX Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | RT | +| Brake | LT | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | LB | +| Turn Signal Right | RB | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | View | +| Clear Emergency Stop | Menu | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | Left Stick Button | +| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml index 8d48948a2d133..73a5d028985c5 100644 --- a/control/joy_controller/config/joy_controller.param.yaml +++ b/control/joy_controller/config/joy_controller.param.yaml @@ -9,6 +9,7 @@ accel_sensitivity: 1.0 brake_sensitivity: 1.0 control_command: + raw_control: false velocity_gain: 3.0 max_forward_velocity: 20.0 max_backward_velocity: 3.0 diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index c6de42afc38d9..22064f9cefaad 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -59,6 +59,7 @@ class AutowareJoyControllerNode : public rclcpp::Node double brake_sensitivity_; // ControlCommand Parameter + bool raw_control_; double velocity_gain_; double max_forward_velocity_; double max_backward_velocity_; diff --git a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp new file mode 100644 index 0000000000000..a009ee1e12975 --- /dev/null +++ b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp @@ -0,0 +1,81 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ + +#include "joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +namespace joy_controller +{ +class XBOXJoyConverter : public JoyConverterBase +{ +public: + explicit XBOXJoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const { return std::max(0.0f, -((RT() - 1.0f) / 2.0f)); } + + float brake() const { return std::max(0.0f, -((LT() - 1.0f) / 2.0f)); } + + float steer() const { return LStickLeftRight(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return LB(); } + bool turn_signal_right() const { return RB(); } + bool clear_turn_signal() const { return A(); } + + bool gate_mode() const { return B(); } + + bool emergency_stop() const { return ChangeView(); } + bool clear_emergency_stop() const { return Menu(); } + + bool autoware_engage() const { return X(); } + bool autoware_disengage() const { return Y(); } + + bool vehicle_engage() const { return LStickButton(); } + bool vehicle_disengage() const { return RStickButton(); } + +private: + float LStickLeftRight() const { return j_.axes.at(0); } + float LStickUpDown() const { return j_.axes.at(1); } + float RStickLeftRight() const { return j_.axes.at(2); } + float RStickUpDown() const { return j_.axes.at(3); } + float RT() const { return j_.axes.at(4); } + float LT() const { return j_.axes.at(5); } + float CursorLeftRight() const { return j_.axes.at(6); } + float CursorUpDown() const { return j_.axes.at(7); } + + bool A() const { return j_.buttons.at(0); } + bool B() const { return j_.buttons.at(1); } + bool X() const { return j_.buttons.at(3); } + bool Y() const { return j_.buttons.at(4); } + bool LB() const { return j_.buttons.at(6); } + bool RB() const { return j_.buttons.at(7); } + bool Menu() const { return j_.buttons.at(11); } + bool LStickButton() const { return j_.buttons.at(13); } + bool RStickButton() const { return j_.buttons.at(14); } + bool ChangeView() const { return j_.buttons.at(15); } + bool Xbox() const { return j_.buttons.at(16); } + + const sensor_msgs::msg::Joy j_; +}; +} // namespace joy_controller + +#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index f719d8bd78cb5..d2804a7339046 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json index c67c575a6bd41..d4c6351324935 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/joy_controller/schema/joy_controller.schema.json @@ -10,7 +10,7 @@ "type": "string", "description": "Joy controller type", "default": "DS4", - "enum": ["P65", "DS4", "G29"] + "enum": ["P65", "DS4", "G29", "XBOX"] }, "update_rate": { "type": "number", @@ -55,6 +55,11 @@ "control_command": { "type": "object", "properties": { + "raw_control": { + "type": "boolean", + "description": "Whether to skip input odometry", + "default": false + }, "velocity_gain": { "type": "number", "description": "Ratio to calculate velocity by acceleration", @@ -79,6 +84,7 @@ } }, "required": [ + "raw_control", "velocity_gain", "max_forward_velocity", "max_backward_velocity", diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 897986a7a41cf..5eec438032410 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -16,6 +16,7 @@ #include "joy_controller/joy_converter/ds4_joy_converter.hpp" #include "joy_controller/joy_converter/g29_joy_converter.hpp" #include "joy_controller/joy_converter/p65_joy_converter.hpp" +#include "joy_controller/joy_converter/xbox_joy_converter.hpp" #include @@ -154,6 +155,8 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt joy_ = std::make_shared(*msg); } else if (joy_type_ == "DS4") { joy_ = std::make_shared(*msg); + } else if (joy_type_ == "XBOX") { + joy_ = std::make_shared(*msg); } else { joy_ = std::make_shared(*msg); } @@ -217,7 +220,7 @@ bool AutowareJoyControllerNode::isDataReady() } // Twist - { + if (!raw_control_) { if (!twist_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), @@ -458,6 +461,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & steering_angle_velocity_ = declare_parameter("steering_angle_velocity"); accel_sensitivity_ = declare_parameter("accel_sensitivity"); brake_sensitivity_ = declare_parameter("brake_sensitivity"); + raw_control_ = declare_parameter("control_command.raw_control"); velocity_gain_ = declare_parameter("control_command.velocity_gain"); max_forward_velocity_ = declare_parameter("control_command.max_forward_velocity"); max_backward_velocity_ = declare_parameter("control_command.max_backward_velocity"); @@ -477,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & sub_joy_ = this->create_subscription( "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); + if (!raw_control_) { + sub_odom_ = this->create_subscription( + "input/odometry", 1, + std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), + subscriber_option); + } else { + twist_ = std::make_shared(); + } // Publisher pub_control_command_ = diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 751eeea66c7b6..281a52a85af71 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -8,7 +8,7 @@ - + @@ -48,4 +48,18 @@ + + + + + + + + + + + + + + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index bda1c84a2747c..87cc510c5fe14 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -32,71 +32,33 @@ This package includes the following features:

-## Launch - -The `ekf_localizer` starts with the default parameters with the following command. - -```sh -roslaunch ekf_localizer ekf_localizer.launch -``` - -The parameters and input topic names can be set in the `ekf_localizer.launch` file. - ## Node ### Subscribed Topics -- measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Input pose source with the measurement covariance matrix. - -- measured_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - Input twist source with the measurement covariance matrix. - -- initialpose (geometry_msgs/PoseWithCovarianceStamped) - - Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. +| Name | Type | Description | +| -------------------------------- | ------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `measured_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose source with the measurement covariance matrix. | +| `measured_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Input twist source with the measurement covariance matrix. | +| `initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. | ### Published Topics -- ekf_odom (nav_msgs/Odometry) - - Estimated odometry. - -- ekf_pose (geometry_msgs/PoseStamped) - - Estimated pose. - -- ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance. - -- ekf_biased_pose (geometry_msgs/PoseStamped) - - Estimated pose including the yaw bias - -- ekf_biased_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance including the yaw bias - -- ekf_twist (geometry_msgs/TwistStamped) - - Estimated twist. - -- ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - The estimated twist with covariance. - -- diagnostics (diagnostic_msgs/DiagnosticArray) - - The diagnostic information. +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ----------------------------------------------------- | +| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | +| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | +| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | +| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | +| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | +| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | +| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | ### Published TF - base_link - - TF from "map" coordinate to estimated pose. + TF from `map` coordinate to estimated pose. ## Functions @@ -205,7 +167,9 @@ Increasing the number will improve the smoothness of the estimation, but may hav -where `b_k` is the yawbias. +where, $\theta_k$ represents the vehicle's heading angle, including the mounting angle bias. +$b_k$ is a correction term for the yaw bias, and it is modeled so that $(\theta_k+b_k)$ becomes the heading angle of the base_link. +The pose_estimator is expected to publish the base_link in the map coordinate system. However, the yaw angle may be offset due to calibration errors. This model compensates this error and improves estimation accuracy. ### time delay model @@ -240,7 +204,7 @@ Note that, although the dimension gets larger since the analytical expansion can ## Known issues -- In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. +- If multiple pose_estimators are used, the input to the EKF will include multiple yaw biases corresponding to each source. However, the current EKF assumes the existence of only one yaw bias. Therefore, yaw bias `b_k` in the current EKF state would not make any sense and cannot correctly handle these multiple yaw biases. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. ## reference diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 573472fe2dabf..8977d82f34138 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -87,9 +87,15 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( { const double x = kalman_filter_.getXelement(IDX::X); const double y = kalman_filter_.getXelement(IDX::Y); + /* + getXelement(IDX::YAW) is surely `biased_yaw`. + Please note how `yaw` and `yaw_bias` are used in the state transition model and + how the observed pose is handled in the measurement pose update. + */ const double biased_yaw = kalman_filter_.getXelement(IDX::YAW); const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB); const double yaw = biased_yaw + yaw_bias; + Pose current_ekf_pose; current_ekf_pose.header.frame_id = params_.pose_frame_id; current_ekf_pose.header.stamp = current_time; @@ -224,7 +230,9 @@ bool EKFModule::measurementUpdatePose( return false; } - /* Set yaw */ + /* Since the kalman filter cannot handle the rotation angle directly, + offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less + than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index ade446020d101..fe65077d89649 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -6,7 +6,6 @@ autoware_package() ament_auto_add_library(localization_util SHARED src/util_func.cpp - src/tf2_listener_module.cpp src/smart_pose_buffer.cpp ) diff --git a/localization/localization_util/include/localization_util/tf2_listener_module.hpp b/localization/localization_util/include/localization_util/tf2_listener_module.hpp deleted file mode 100644 index b332f9effe0e0..0000000000000 --- a/localization/localization_util/include/localization_util/tf2_listener_module.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ -#define LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ - -#include - -#include -#include -#include - -#include - -class Tf2ListenerModule -{ - using TransformStamped = geometry_msgs::msg::TransformStamped; - -public: - explicit Tf2ListenerModule(rclcpp::Node * node); - bool get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr) const; - -private: - rclcpp::Logger logger_; - tf2_ros::Buffer tf2_buffer_; - tf2_ros::TransformListener tf2_listener_; -}; - -#endif // LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ diff --git a/localization/localization_util/src/tf2_listener_module.cpp b/localization/localization_util/src/tf2_listener_module.cpp deleted file mode 100644 index 8a19ceec9f30d..0000000000000 --- a/localization/localization_util/src/tf2_listener_module.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 "localization_util/tf2_listener_module.hpp" - -#include - -geometry_msgs::msg::TransformStamped identity_transform_stamped( - const builtin_interfaces::msg::Time & timestamp, const std::string & header_frame_id, - const std::string & child_frame_id) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = timestamp; - transform.header.frame_id = header_frame_id; - transform.child_frame_id = child_frame_id; - transform.transform.rotation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); - transform.transform.translation = tier4_autoware_utils::createTranslation(0.0, 0.0, 0.0); - return transform; -} - -Tf2ListenerModule::Tf2ListenerModule(rclcpp::Node * node) -: logger_(node->get_logger()), tf2_buffer_(node->get_clock()), tf2_listener_(tf2_buffer_) -{ -} - -bool Tf2ListenerModule::get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, const TransformStamped::SharedPtr & transform_stamped_ptr) const -{ - const TransformStamped identity = - identity_transform_stamped(timestamp, target_frame, source_frame); - - if (target_frame == source_frame) { - *transform_stamped_ptr = identity; - return true; - } - - try { - *transform_stamped_ptr = - tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(logger_, "%s", ex.what()); - RCLCPP_ERROR(logger_, "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - *transform_stamped_ptr = identity; - return false; - } - return true; -} diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 2ad4a47b833d1..b4d656cb1810b 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -30,7 +30,6 @@ ament_auto_add_executable(ndt_scan_matcher src/particle.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp - src/map_module.cpp src/map_update_module.cpp ) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index d7a7b1c5c37f3..95a7cebdc01c8 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -18,7 +18,6 @@ One optional function is regularization. Please see the regularization chapter i | Name | Type | Description | | ----------------------------------- | ----------------------------------------------- | ------------------------------------- | | `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | initial pose | -| `pointcloud_map` | `sensor_msgs::msg::PointCloud2` | map pointcloud | | `points_raw` | `sensor_msgs::msg::PointCloud2` | sensor pointcloud | | `sensing/gnss/pose_with_covariance` | `sensor_msgs::msg::PoseWithCovarianceStamped` | base position for regularization term | @@ -193,12 +192,6 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Additional interfaces -#### Additional inputs - -| Name | Type | Description | -| ---------------- | ------------------------- | ----------------------------------------------------------- | -| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | - #### Additional outputs | Name | Type | Description | @@ -213,20 +206,15 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | -------------------------------------------------------------------- | -| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | -| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | -| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | -| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | - -### Enabling the dynamic map loading feature +| Name | Type | Description | +| ------------------------------------- | ------ | ------------------------------------------------------------ | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | -To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. -Follow the next two instructions. +### Notes for dynamic map loading -1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) -2. split the PCD files into grids (recommended size: 20[m] x 20[m]) +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to split the PCD files into grids (recommended size: 20[m] x 20[m]) Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of @@ -235,14 +223,10 @@ Note that the dynamic map loading may FAIL if the map is split into two or more Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) -| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | -| :------------: | :-----------------------: | :------------------------: | :------------------: | -| single file | true | true | at once (standard) | -| single file | true | false | **does NOT work** | -| single file | false | true/false | at once (standard) | -| multiple files | true | true | dynamically | -| multiple files | true | false | **does NOT work** | -| multiple files | false | true/false | at once (standard) | +| PCD files | How NDT loads map(s) | +| :------------: | :------------------: | +| single file | at once (standard) | +| multiple files | dynamically | ## Scan matching score based on de-grounded LiDAR scan diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 1a6c26cd9c351..9bc62d3f919c6 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,8 +1,5 @@ /**: ros__parameters: - # Use dynamic map loading - use_dynamic_map_loading: true - # Vehicle reference frame base_frame: "base_link" diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp deleted file mode 100644 index 79454e89b9ed0..0000000000000 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 NDT_SCAN_MATCHER__MAP_MODULE_HPP_ -#define NDT_SCAN_MATCHER__MAP_MODULE_HPP_ - -#include - -#include - -#include -#include -#include - -#include - -class MapModule -{ - using PointSource = pcl::PointXYZ; - using PointTarget = pcl::PointXYZ; - using NormalDistributionsTransform = - pclomp::MultiGridNormalDistributionsTransform; - -public: - MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group); - -private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); - - rclcpp::Subscription::SharedPtr map_points_sub_; - std::shared_ptr ndt_ptr_; - std::mutex * ndt_ptr_mutex_; -}; - -#endif // NDT_SCAN_MATCHER__MAP_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 5457a6c310f5d..8b192b0186b42 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -15,7 +15,6 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "localization_util/tf2_listener_module.hpp" #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" 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 6a5c61041f0da..e9aa265c78f34 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 @@ -18,8 +18,6 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" -#include "localization_util/tf2_listener_module.hpp" -#include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" #include @@ -41,6 +39,8 @@ #include #include #include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -176,6 +176,8 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Service::SharedPtr service_trigger_node_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; rclcpp::CallbackGroup::SharedPtr timer_callback_group_; @@ -213,8 +215,6 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::shared_ptr tf2_listener_module_; - std::unique_ptr map_module_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; @@ -222,8 +222,6 @@ class NDTScanMatcher : public rclcpp::Node bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; - bool use_dynamic_map_loading_; - // The execution time which means probably NDT cannot matches scans properly int64_t critical_upper_bound_exe_time_ms_; }; diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 655aa8e17ccd9..891d172aaf8fd 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -4,7 +4,6 @@ - @@ -19,7 +18,6 @@ - diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp deleted file mode 100644 index d6088a1b14949..0000000000000 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 "ndt_scan_matcher/map_module.hpp" - -MapModule::MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex) -{ - auto map_sub_opt = rclcpp::SubscriptionOptions(); - map_sub_opt.callback_group = map_callback_group; - - map_points_sub_ = node->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MapModule::callback_map_points, this, std::placeholders::_1), map_sub_opt); -} - -void MapModule::callback_map_points( - sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) -{ - NormalDistributionsTransform new_ndt; - new_ndt.setParams(ndt_ptr_->getParams()); - - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); - new_ndt.setInputTarget(map_points_ptr); - // create Thread - // detach - auto output_cloud = std::make_shared>(); - new_ndt.align(*output_cloud); - - // swap - ndt_ptr_mutex_->lock(); - *ndt_ptr_ = new_ndt; - ndt_ptr_mutex_->unlock(); -} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 446f926a1d3f7..39b92fe248660 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -57,10 +57,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->cached_ids = ndt_ptr_->getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - logger_, - "Waiting for pcd loader service. Check if the enable_differential_load in " - "pointcloud_map_loader is set `true`."); + RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); } // send a request to map_loader 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 7f6fe605c42b0..0f6fdbc15db26 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -68,6 +68,8 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), + tf2_buffer_(this->get_clock()), + tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), output_pose_covariance_({}), @@ -262,14 +264,7 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - tf2_listener_module_ = std::make_shared(this); - - use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); - if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); - } else { - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, sensor_callback_group); - } + map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); logger_configure_ = std::make_unique(this); } @@ -347,9 +342,6 @@ void NDTScanMatcher::callback_timer() if (!is_activated_) { return; } - if (!use_dynamic_map_loading_) { - return; - } std::lock_guard lock(latest_ekf_position_mtx_); if (latest_ekf_position_ == std::nullopt) { RCLCPP_ERROR_STREAM_THROTTLE( @@ -380,7 +372,8 @@ void NDTScanMatcher::callback_initial_pose( << ". Please check the frame_id in the input topic and ensure it is correct."); } - if (use_dynamic_map_loading_) { + { + // latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock std::lock_guard lock(latest_ekf_position_mtx_); latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position; } @@ -574,11 +567,25 @@ void NDTScanMatcher::transform_sensor_measurement( const pcl::shared_ptr> & sensor_points_input_ptr, pcl::shared_ptr> & sensor_points_output_ptr) { - auto tf_target_to_source_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - this->now(), target_frame, source_frame, tf_target_to_source_ptr); + if (source_frame == target_frame) { + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + // Since there is no clear error handling policy, temporarily return as is. + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - tier4_autoware_utils::transform2pose(*tf_target_to_source_ptr); + tier4_autoware_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); tier4_autoware_utils::transformPointCloud( @@ -863,17 +870,29 @@ void NDTScanMatcher::service_ndt_align( tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { // get TF from pose_frame to map_frame - auto tf_pose_to_map_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - get_clock()->now(), map_frame_, req->pose_with_covariance.header.frame_id, tf_pose_to_map_ptr); + const std::string & target_frame = map_frame_; + const std::string & source_frame = req->pose_with_covariance.header.frame_id; - // transform pose_frame to map_frame - const auto initial_pose_msg_in_map_frame = - transform(req->pose_with_covariance, *tf_pose_to_map_ptr); - if (use_dynamic_map_loading_) { - map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + geometry_msgs::msg::TransformStamped transform_s2t; + try { + transform_s2t = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + // Note: Up to AWSIMv1.1.0, there is a known bug where the GNSS frame_id is incorrectly set to + // "gnss_link" instead of "map". The ndt_align is designed to return identity when this issue + // occurs. However, in the future, converting to a non-existent frame_id should be prohibited. + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + transform_s2t.header.stamp = get_clock()->now(); + transform_s2t.header.frame_id = target_frame; + transform_s2t.child_frame_id = source_frame; + transform_s2t.transform = tf2::toMsg(tf2::Transform::getIdentity()); } + // transform pose_frame to map_frame + const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + // mutex Map std::lock_guard lock(ndt_ptr_mtx_); diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 1eb1edd04a68a..a05c63d44ebce 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -101,11 +101,11 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) req->area.center_y = point.y; req->area.radius = 50; - RCLCPP_INFO(logger, "Send request to map_loader"); + RCLCPP_DEBUG(logger, "Send request to map_loader"); auto future = cli_map_->async_send_request(req); auto status = future.wait_for(std::chrono::seconds(1)); while (status != std::future_status::ready) { - RCLCPP_INFO(logger, "waiting response"); + RCLCPP_DEBUG(logger, "waiting response"); if (!rclcpp::ok()) { return false; } @@ -113,7 +113,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } const auto res = future.get(); - RCLCPP_INFO( + RCLCPP_DEBUG( logger, "Loaded partial pcd map from map_loader (grid size: %lu)", res->new_pointcloud_with_ids.size()); @@ -168,7 +168,7 @@ std::optional MapHeightFitter::Impl::fit(const Point & position, const st const auto logger = node_->get_logger(); tf2::Vector3 point(position.x, position.y, position.z); - RCLCPP_INFO(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); if (cli_map_) { if (!get_partial_point_cloud_map(position)) { @@ -193,7 +193,7 @@ std::optional MapHeightFitter::Impl::fit(const Point & position, const st return std::nullopt; } - RCLCPP_INFO(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); Point result; result.x = point.getX(); diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 65d9594cb7415..fbe019096a3e7 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,7 +20,7 @@ NOTE: **We strongly recommend to use divided maps when using large pointcloud ma #### Prerequisites on pointcloud map file(s) -You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules: +You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. @@ -29,8 +29,6 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. -Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023). - #### Metadata structure The metadata should look like this: @@ -118,7 +116,6 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | | enable_partial_load | bool | A flag to enable partial pointcloud map server | false | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | | enable_selected_load | bool | A flag to enable selected pointcloud map server | false | | leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | | pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index ba4c032d3e514..b4efbec9706b4 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,7 +3,6 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: true - enable_differential_load: true enable_selected_load: false # only used when downsample_whole_load enabled diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index a2d9307084545..5f4b3e311e6e9 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -53,7 +53,6 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); - bool enable_differential_load = declare_parameter("enable_differential_load"); bool enable_selected_load = declare_parameter("enable_selected_load"); if (enable_whole_load) { @@ -68,26 +67,21 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load || enable_differential_load || enable_selected_load) { - std::map pcd_metadata_dict; - try { - pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } + std::map pcd_metadata_dict; + try { + pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); + } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict); - } + differential_map_loader_ = std::make_unique(this, pcd_metadata_dict); - if (enable_selected_load) { - selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_selected_load) { + selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); } } diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 16442eb43c740..54e794e2742bf 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -23,6 +23,8 @@ #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename); class MapProjectionLoader : public rclcpp::Node { diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 9fdba288601cc..5966baaed8383 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -20,6 +20,7 @@ #include +#include #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) @@ -55,28 +56,40 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename) { - std::string yaml_filename = this->declare_parameter("map_projector_info_path"); - std::string lanelet2_map_filename = this->declare_parameter("lanelet2_map_path"); - std::ifstream file(yaml_filename); - tier4_map_msgs::msg::MapProjectorInfo msg; - bool use_yaml_file = file.is_open(); - if (use_yaml_file) { - RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); + if (std::filesystem::exists(yaml_filename)) { + std::cout << "Load " << yaml_filename << std::endl; msg = load_info_from_yaml(yaml_filename); - } else { - RCLCPP_INFO(this->get_logger(), "Load %s", lanelet2_map_filename.c_str()); - RCLCPP_WARN( - this->get_logger(), - "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " - "Please use map_projector_info.yaml instead. For more info, visit " - "https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/" - "README.md"); + } else if (std::filesystem::exists(lanelet2_map_filename)) { + std::cout << "Load " << lanelet2_map_filename << std::endl; + std::cout + << "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/" + "map_projection_loader/" + "README.md" + << std::endl; msg = load_info_from_lanelet2_map(lanelet2_map_filename); + } else { + throw std::runtime_error( + "No map projector info files found. Please provide either " + "map_projector_info.yaml or lanelet2_map.osm"); } + return msg; +} + +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +{ + const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); + const std::string lanelet2_map_filename = + this->declare_parameter("lanelet2_map_path"); + + const tier4_map_msgs::msg::MapProjectorInfo msg = + load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message const auto adaptor = component_interface_utils::NodeAdaptor(this); diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index c021dd92dae64..b13f68b07181e 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/segmentation_pointcloud_fusion/node.cpp src/roi_pointcloud_fusion/node.cpp ) @@ -40,6 +41,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::SegmentPointCloudFusionNode" + EXECUTABLE segmentation_pointcloud_fusion_node +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" EXECUTABLE roi_pointcloud_fusion_node diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000000..914ad13519807 --- /dev/null +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -0,0 +1,30 @@ +/**: + ros__parameters: + # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: + [ + true, # road + true, # sidewalk + true, # building + true, # wall + true, # fence + true, # pole + true, # traffic_light + true, # traffic_sign + true, # vegetation + true, # terrain + true, # sky + false, # person + false, # ride + false, # car + false, # truck + false, # bus + false, # train + false, # motorcycle + false, # bicycle + false, # others + ] + # the maximum distance of pointcloud to be applied filter, + # this is selected based on semantic segmentation model accuracy, + # calibration accuracy and unknown reaction distance + filter_distance_threshold: 60.0 diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md new file mode 100644 index 0000000000000..77188aafb3b22 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -0,0 +1,87 @@ +# segmentation_pointcloud_fusion + +## Purpose + +The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud that are belong to less interesting region which is defined by semantic or instance segmentation by 2D image segmentation model. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto the semantic/instance segmentation mask image which is the output from 2D segmentation neural network. +- The pointclouds are belong to segment such as road, sidewalk, building, vegetation, sky ... are considered as less important points for autonomous driving and could be removed. + +![segmentation_pointcloud_fusion_image](./images/segmentation_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | ----------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::Image` | semantic segmentation mask image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| -------- | ------------------------------- | -------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output filtered pointcloud | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ------------- | ---- | ------------------------ | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 9572e62e0e0aa..fe8acb6746dba 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -49,13 +50,14 @@ namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using sensor_msgs::msg::CameraInfo; +using sensor_msgs::msg::Image; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; using autoware_auto_perception_msgs::msg::ObjectClassification; - -template +template class FusionNode : public rclcpp::Node { public: @@ -78,12 +80,12 @@ class FusionNode : public rclcpp::Node virtual void subCallback(const typename Msg::ConstSharedPtr input_msg); // callback for roi subscription + virtual void roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); virtual void fuseOnSingleImage( - const Msg & input_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, + const Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0; // set args if you need @@ -111,7 +113,7 @@ class FusionNode : public rclcpp::Node /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; - std::vector::SharedPtr> rois_subs_; + std::vector::SharedPtr> rois_subs_; // offsets between cameras and the lidars std::vector input_offset_ms_; @@ -120,7 +122,7 @@ class FusionNode : public rclcpp::Node std::vector is_fused_; std::pair cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; + std::vector> cached_roi_msgs_; std::mutex mutex_cached_msgs_; // output publisher diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index d7de10fed068f..78ae152141a00 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -33,7 +33,8 @@ namespace image_projection_based_fusion { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; -class PointPaintingFusionNode : public FusionNode +class PointPaintingFusionNode +: public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index e54710ad477da..b9471f2f3b78e 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode -: public FusionNode +: public FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index e11a62c060480..7d7132309fb91 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -29,7 +29,8 @@ namespace image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode : public FusionNode +class RoiDetectedObjectFusionNode +: public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 2b4eb822a9edb..fe685baa0b6e2 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -22,7 +22,7 @@ namespace image_projection_based_fusion { class RoiPointCloudFusionNode -: public FusionNode +: public FusionNode { private: int min_cluster_size_{1}; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..4168c483469ab --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +namespace image_projection_based_fusion +{ +class SegmentPointCloudFusionNode : public FusionNode +{ +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + std::vector filter_semantic_label_target_; + float filter_distance_threshold_; + /* data */ +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(PointCloud2 & pointcloud_msg) override; + + void postprocess(PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, + const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + + bool out_of_scope(const PointCloud2 & filtered_cloud); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..96bf47594bfe8 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 4abd92d7fb063..6bced39bc61ef 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -40,8 +40,8 @@ static double processing_time_ms = 0; namespace image_projection_based_fusion { -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -81,7 +81,7 @@ FusionNode::FusionNode( } input_offset_ms_ = declare_parameter>("input_offset_ms"); - if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { + if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { throw std::runtime_error("The number of offsets does not match the number of topics."); } @@ -99,9 +99,9 @@ FusionNode::FusionNode( cached_roi_msgs_.resize(rois_number_); is_fused_.resize(rois_number_, false); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - std::function roi_callback = + std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); - rois_subs_.at(roi_i) = this->create_subscription( + rois_subs_.at(roi_i) = this->create_subscription( input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } @@ -147,22 +147,22 @@ FusionNode::FusionNode( filter_scope_maxz_ = declare_parameter("filter_scope_max_z"); } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { camera_info_map_[camera_id] = *input_camera_info_msg; } -template -void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) +template +void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) +template +void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -285,9 +285,9 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ } } -template -void FusionNode::roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { stop_watch_ptr_->toc("processing_time", true); @@ -350,14 +350,14 @@ void FusionNode::roiCallback( (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; } -template -void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) +template +void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::timer_callback() +template +void FusionNode::timer_callback() { using std::chrono_literals::operator""ms; timer_->cancel(); @@ -395,8 +395,8 @@ void FusionNode::timer_callback() } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -412,8 +412,8 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const Msg & output_msg) +template +void FusionNode::publish(const Msg & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -421,8 +421,10 @@ void FusionNode::publish(const Msg & output_msg) pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode; -template class FusionNode; -template class FusionNode; +template class FusionNode; +template class FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; +template class FusionNode; +template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 76b561f677c0f..14783a46ba8b4 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -93,7 +93,8 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("pointpainting_fusion", options) +: FusionNode( + "pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 492f832e30e8b..1c064e4d0f060 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -34,7 +34,8 @@ namespace image_projection_based_fusion { RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_cluster_fusion", options) +: FusionNode( + "roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 333ec7d7ed206..19defc0a1cab0 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion { RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_detected_object_fusion", options) +: FusionNode( + "roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 233e568ebee0b..421aa3a453451 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -30,7 +30,8 @@ namespace image_projection_based_fusion { RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_pointcloud_fusion", options) +: FusionNode( + "roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..1c9c865b8d21e --- /dev/null +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -0,0 +1,139 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +namespace image_projection_based_fusion +{ +SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("segmentation_pointcloud_fusion", options) +{ + filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); + filter_semantic_label_target_ = + declare_parameter>("filter_semantic_label_target"); +} + +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} + +void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} +void SegmentPointCloudFusionNode::fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, + [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, + __attribute__((unused)) PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + cv_bridge::CvImagePtr in_image_ptr; + try { + in_image_ptr = cv_bridge::toCvCopy( + std::make_shared(input_mask), input_mask.encoding); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception:%s", e.what()); + return; + } + + cv::Mat mask = in_image_ptr->image; + if (mask.cols == 0 || mask.rows == 0) { + return; + } + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, + 0.0, 1.0; + geometry_msgs::msg::TransformStamped transform_stamped; + // transform pointcloud from frame id to camera optical frame id + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_mask.header.frame_id, input_pointcloud_msg.header.frame_id, + input_pointcloud_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + PointCloud output_cloud; + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + // skip filtering pointcloud behind the camera or too far from camera + if (*iter_z <= 0.0 || *iter_z > filter_distance_threshold_) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + bool is_inside_image = + normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width && + normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height; + if (!is_inside_image) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + // skip filtering pointcloud where semantic id out of the defined list + uint8_t semantic_id = mask.at( + static_cast(normalized_projected_point.y()), + static_cast(normalized_projected_point.x())); + if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + if (!filter_semantic_label_target_.at(semantic_id)) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + + pcl::toROSMsg(output_cloud, output_pointcloud_msg); + output_pointcloud_msg.header = input_pointcloud_msg.header; +} + +bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const PointCloud2 & filtered_cloud) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::SegmentPointCloudFusionNode) diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 0091fc6bc38cb..75c1d61e0a19c 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -415,17 +415,21 @@ bool withinRoadLanelet( } boost::optional isReachableCrosswalkEdgePoints( - const TrackedObject & object, const CrosswalkEdgePoints & edge_points, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const double time_horizon, - const double min_object_vel) + const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk, + const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const double time_horizon, const double min_object_vel) { using Point = boost::geometry::model::d2::point_xy; - using Line = boost::geometry::model::linestring; const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); + if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { + return {}; + } + const auto & p1 = edge_points.front_center_point; const auto & p2 = edge_points.back_center_point; @@ -442,62 +446,66 @@ boost::optional isReachableCrosswalkEdgePoints( const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet const auto surrounding_lanelets = lanelet::geometry::findNearest( - lanelet_map_ptr->laneletLayer, search_point, time_horizon * velocity); + lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity); - bool first_intersect_load = false; - bool second_intersect_load = false; - std::vector intersects_first; - std::vector intersects_second; - for (const auto & lanelet : surrounding_lanelets) { - if (withinLanelet(object, lanelet.second)) { - return {}; - } + const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) { + const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) { + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if ( + (attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) && + boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; - lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); - if (attr.value() != "road") { - continue; - } + const auto isExist = [](const Point & p, const std::vector & points) { + for (const auto & existingPoint : points) { + if (boost::geometry::distance(p, existingPoint) < 1e-1) { + return true; + } + } + return false; + }; - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}}; - std::vector tmp_intersects; - boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); - for (const auto & p : tmp_intersects) { - intersects_first.push_back(p); + std::vector points_of_intersect; + const boost::geometry::model::linestring line{p_src, p_dst}; + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if (attr.value() != lanelet::AttributeValueString::Road) { + continue; } - } - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}}; std::vector tmp_intersects; boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); + line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); for (const auto & p : tmp_intersects) { - intersects_second.push_back(p); + if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) { + continue; + } + points_of_intersect.push_back(p); + if (points_of_intersect.size() >= 2) { + return true; + } } } - } - - if (1 < intersects_first.size()) { - first_intersect_load = true; - } + return false; + }; - if (1 < intersects_second.size()) { - second_intersect_load = true; - } + const bool first_intersects_road = isAcrossAnyRoad( + {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}); + const bool second_intersects_road = + isAcrossAnyRoad({obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}); - if (first_intersect_load && second_intersect_load) { + if (first_intersects_road && second_intersects_road) { return {}; } - if (first_intersect_load && !second_intersect_load) { + if (first_intersects_road && !second_intersects_road) { ret.swap(); } @@ -1197,48 +1205,45 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( predicted_object.kinematics.predicted_paths.push_back(predicted_path); } } + } + // try to find the edge points for all crosswalks and generate path to the crosswalk edge + for (const auto & crosswalk : crosswalks_) { + const auto edge_points = getCrosswalkEdgePoints(crosswalk); + + const auto reachable_first = hasPotentialToReach( + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + const auto reachable_second = hasPotentialToReach( + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + + if (!reachable_first && !reachable_second) { + continue; + } - // If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge - // points for all crosswalks and generate path to the crosswalk edge - } else { - for (const auto & crosswalk : crosswalks_) { - const auto edge_points = getCrosswalkEdgePoints(crosswalk); - - const auto reachable_first = hasPotentialToReach( - object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - const auto reachable_second = hasPotentialToReach( - object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - - if (!reachable_first && !reachable_second) { - continue; - } - - const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, edge_points, lanelet_map_ptr_, prediction_time_horizon_, - min_crosswalk_user_velocity_); - - if (!reachable_crosswalk) { - continue; - } + const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + min_crosswalk_user_velocity_); - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); - predicted_path.confidence = 1.0; + if (!reachable_crosswalk) { + continue; + } - if (predicted_path.path.empty()) { - continue; - } - // If the predicted path to the crosswalk is crossing the fence, don't use it - if (doesPathCrossAnyFence(predicted_path)) { - continue; - } + PredictedPath predicted_path = + path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); + if (predicted_path.path.empty()) { + continue; } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; + } + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); } const auto n_path = predicted_object.kinematics.predicted_paths.size(); diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 3d2e4662f7ea5..763bd12fab79e 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -13,6 +13,7 @@ eigen3_cmake_module autoware_auto_perception_msgs + diagnostic_updater eigen kalman_filter libgoogle-glog-dev diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 035845772ce53..928532f928e38 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -55,6 +55,24 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map EXECUTABLE laserscan_based_occupancy_grid_map_node ) +# GridMapFusionNode +ament_auto_add_library(synchronized_grid_map_fusion SHARED + src/fusion/synchronized_grid_map_fusion_node.cpp + src/fusion/single_frame_fusion_policy.cpp + src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp + src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp + src/utils/utils.cpp +) + +target_link_libraries(synchronized_grid_map_fusion + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(synchronized_grid_map_fusion + PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + EXECUTABLE synchronized_grid_map_fusion_node +) + ament_auto_package( INSTALL_TO_SHARE launch @@ -66,13 +84,22 @@ if(BUILD_TESTING) # launch_testing find_package(launch_testing_ament_cmake REQUIRED) add_launch_test(test/test_pointcloud_based_method.py) + add_launch_test(test/test_synchronized_grid_map_fusion_node.py) # gtest ament_add_gtest(test_utils test/test_utils.cpp ) + ament_add_gtest(costmap_unit_tests + test/cost_value_test.cpp) + ament_add_gtest(fusion_policy_unit_tests + test/fusion_policy_test.cpp + src/fusion/single_frame_fusion_policy.cpp + ) target_link_libraries(test_utils ${PCL_LIBRARIES} ${PROJECT_NAME}_common ) + target_include_directories(costmap_unit_tests PRIVATE "include") + target_include_directories(fusion_policy_unit_tests PRIVATE "include") endif() diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 962bcdc6f154f..084c55d80d629 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -9,6 +9,7 @@ This package outputs the probability of having an obstacle as occupancy grid map - [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md) - [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md) +- [Grid map fusion](synchronized_grid_map_fusion.md) ## Settings @@ -70,3 +71,19 @@ Additional argument is shown below: | `container_name` | `occupancy_grid_map_container` | | | `input_obstacle_pointcloud` | `false` | only for laserscan based method. If true, the node subscribe obstacle pointcloud | | `input_obstacle_and_raw_pointcloud` | `true` | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud | + +### Test + +This package provides unit tests using `gtest`. +You can run the test by the following command. + +```bash +colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+ +``` + +Test contains the following. + +- Unit test for cost value conversion function +- Unit test for utility functions +- Unit test for occupancy grid map fusion functions +- Input/Output test for pointcloud based occupancy grid map diff --git a/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml b/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml new file mode 100644 index 0000000000000..f8a2dc2fbc7de --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # 1. fusion parameters + fusion_input_ogm_topics: ["topic1", "topic2"] + input_ogm_reliabilities: [0.8, 0.2] + fusion_method: "overwrite" # choose from ["overwrite", "log-odds", "dempster-shafer"] + + # 2. synchronization settings + match_threshold_sec: 0.01 # 10ms + timeout_sec: 0.1 # 100ms + input_offset_sec: [0.0, 0.0] # no offset + + # 3. settings for fused fusion map + # remember resolution and map size should be same with input maps + map_frame_: "map" + base_link_frame_: "base_link" + grid_map_origin_frame_: "base_link" + fusion_map_length_x: 100.0 + fusion_map_length_y: 100.0 + fusion_map_resolution: 0.5 diff --git a/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg b/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg new file mode 100644 index 0000000000000..6bf5ed98e4edf --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg @@ -0,0 +1,290 @@ + + + + + + + + + + +
+
+
+ OGM1 +
+
+
+ + OGM1 + + + + + + + + +
+
+
+ OGM2 +
+
+
+ + OGM2 + + + + + + + + +
+
+
+ OGM3 +
+
+
+ + OGM3 + + + + + + + + +
+
+
+ Fused +
+ OGM +
+
+
+ + Fused... + + + + + + + + +
+
+
+ Fused +
+ OGM +
+ (Single Frame) +
+
+
+ + Fused... + + + + + + + + +
+
+
+ + Single Frame +
+ Fusion + +
+
+
+ + Single Frame... + + + + + + + + +
+
+
+ + Multi Frame +
+ Fusion + +
+
+
+ + Multi Frame... + + + + + + + + +
+
+
+ Fused +
+ OGM +
+
+
+ + Fused... + + + + + + +
+
+
`t=t_n`
+
+
+ + `t=t_n` + + + + + + +
+
+
`t=t_{n-1}`
+
+
+ + `t=t_{n-1}` + + + + + + +
+
+
`t=t_n`
+
+
+ + `t=t_n` + + + + + + +
+
+
+ Publish +
+
+
+ + Publish + + + + + + +
+
+
+ input topics +
+
+
+ + input topics + + + + + + +
+
+
+ output topics +
+
+
+ + output topics + + + + + + +
+
+
+ grid_map_fusion::GridMapFusionNode +
+
+
+ + grid_map_fusion::GridMapFusionNode + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp index e9667ccf65577..e0f18cedcdff1 100644 --- a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp @@ -59,19 +59,48 @@ static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; static const unsigned char FREE_SPACE = 0; +static const unsigned char OCCUPIED_THRESHOLD = 180; +static const unsigned char FREE_THRESHOLD = 50; + struct CostTranslationTable { CostTranslationTable() { for (int i = 0; i < 256; i++) { - const auto value = static_cast(static_cast(i) * 100.f / 255.f); - data[i] = std::max(std::min(value, static_cast(99)), static_cast(1)); + const auto value = + static_cast(static_cast(i) * 100.f / 255.f); // 0-255 to 0-100 + data[i] = + std::max(std::min(value, static_cast(99)), static_cast(1)); // 0-100 to 1-99 } } char operator[](unsigned char n) const { return data[n]; } char data[256]; }; +struct InverseCostTranslationTable +{ + InverseCostTranslationTable() + { + // 0-100 to 0-255 + for (int i = 0; i < 100; i++) { + data[i] = static_cast(i * 255 / 99); + } + } + unsigned char operator[](char n) const + { + if (n > 99) { + return data[99]; + } else if (n < 1) { + return data[1]; + } else { + const unsigned char u_n = static_cast(n); + return data[u_n]; + } + } + unsigned char data[100]; +}; + static const CostTranslationTable cost_translation_table; +static const InverseCostTranslationTable inverse_cost_translation_table; } // namespace occupancy_cost_value #endif // COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp new file mode 100644 index 0000000000000..13829b4910d96 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp @@ -0,0 +1,66 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#define FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ + +#include "cost_value.hpp" + +#include +#include +#include +#include + +/*min and max prob threshold to prevent log-odds to diverge*/ +#define EPSILON_PROB 0.03 + +namespace fusion_policy +{ +enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; + +unsigned char convertProbabilityToChar(const double & probability); +double convertCharToProbability(const unsigned char & occupancy); +std::vector convertProbabilityToChar(const std::vector & probabilities); +std::vector convertCharToProbability(const std::vector & occupancies); + +namespace overwrite_fusion +{ +enum State : unsigned char { UNKNOWN = 0U, FREE = 1U, OCCUPIED = 2U }; +State getApproximateState(const unsigned char & occupancy); +unsigned char overwriteFusion(const std::vector & occupancies); +} // namespace overwrite_fusion + +namespace log_odds_fusion +{ +double logOddsFusion(const std::vector & probabilities); +double logOddsFusion( + const std::vector & probabilities, const std::vector & weights); +} // namespace log_odds_fusion + +namespace dempster_shafer_fusion +{ +struct dempsterShaferOccupancy; +double dempsterShaferFusion(const std::vector & probability); +double dempsterShaferFusion( + const std::vector & probability, const std::vector & reliability); +} // namespace dempster_shafer_fusion + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method); +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method, + const std::vector & reliability); +} // namespace fusion_policy + +#endif // FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp new file mode 100644 index 0000000000000..6bea5b2a16f72 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp @@ -0,0 +1,127 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ + +#include "fusion/single_frame_fusion_policy.hpp" +#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "updater/occupancy_grid_map_updater_interface.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace synchronized_grid_map_fusion +{ + +using costmap_2d::OccupancyGridMapFixedBlindSpot; +using costmap_2d::OccupancyGridMapLOBFUpdater; +using costmap_2d::OccupancyGridMapUpdaterInterface; +using geometry_msgs::msg::Pose; + +class GridMapFusionNode : public rclcpp::Node +{ +public: + explicit GridMapFusionNode(const rclcpp::NodeOptions & node_options); + +private: + void onGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, + const std::string & input_topic); + nav_msgs::msg::OccupancyGrid::UniquePtr OccupancyGridMapToMsgPtr( + const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map); + + nav2_costmap_2d::Costmap2D OccupancyGridMsgToCostmap2D( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); + OccupancyGridMapFixedBlindSpot OccupancyGridMsgToGridMap( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); + OccupancyGridMapFixedBlindSpot SingleFrameOccupancyFusion( + std::vector & occupancy_grid_maps, + const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights); + + void updateGridMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg); + + void setPeriod(const int64_t new_period); + void timer_callback(); + void publish(); + +private: + // Publisher and Subscribers + rclcpp::Publisher::SharedPtr fused_map_pub_; + rclcpp::Publisher::SharedPtr single_frame_pub_; + std::vector::SharedPtr> grid_map_subs_; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; + + // Topics manager + std::size_t num_input_topics_{1}; + std::vector input_topics_; + std::vector input_offset_sec_; + std::vector input_topic_weights_; + std::map input_topic_weights_map_; + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // Timer to manage the timeout of the occupancy grid map + rclcpp::TimerBase::SharedPtr timer_; + double timeout_sec_{}; + double match_threshold_sec_{}; + + // cache for fusion + std::mutex mutex_; + std::shared_ptr + occupancy_grid_map_updater_ptr_; // contains fused grid map + std::map + gridmap_dict_; // temporary cache for grid map message + std::map + gridmap_dict_tmp_; // second cache for grid map message + std::map offset_map_; // time offset for each grid map + + // grid map parameters + std::string map_frame_; + std::string base_link_frame_; + std::string gridmap_origin_frame_; + float fusion_map_length_x_; + float fusion_map_length_y_; + float fusion_map_resolution_; + fusion_policy::FusionMethod fusion_method_; +}; + +} // namespace synchronized_grid_map_fusion + +#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp new file mode 100644 index 0000000000000..306f8988acab7 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ + +#include "fusion/single_frame_fusion_policy.hpp" +#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "utils/utils.hpp" + +#include +#include + +// LOBF means: Log Odds Bayes Filter + +namespace costmap_2d +{ +class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface +{ +public: + enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; + OccupancyGridMapLOBFUpdater( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) + : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) + { + } + bool update(const Costmap2D & single_frame_occupancy_grid_map) override; + void initRosParam(rclcpp::Node & node) override; + +private: + inline unsigned char applyLOBF(const unsigned char & z, const unsigned char & o); + Eigen::Matrix2f probability_matrix_; +}; + +} // namespace costmap_2d + +#endif // UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp index 047b747c2861f..46bd14b843dae 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp @@ -15,6 +15,8 @@ #ifndef UTILS__UTILS_HPP_ #define UTILS__UTILS_HPP_ +#include "cost_value.hpp" + #include #include #include @@ -114,6 +116,7 @@ bool extractCommonPointCloud( const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); +unsigned char getApproximateOccupancyState(const unsigned char & value); } // namespace utils #endif // UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml b/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml new file mode 100644 index 0000000000000..c2391141e9fa0 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp new file mode 100644 index 0000000000000..75e8791a04bca --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp @@ -0,0 +1,322 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "fusion/single_frame_fusion_policy.hpp" + +namespace fusion_policy +{ + +//// utils //// + +// convert [0, 1] to [0, 255] +unsigned char convertProbabilityToChar(const double & probability) +{ + return static_cast(probability * 255); +} +// convert [0, 255] to [0, 1] +double convertCharToProbability(const unsigned char & occupancy) +{ + return static_cast(occupancy) / 255.0; +} + +// convert [0, 1] to [0, 255] vectors +std::vector convertProbabilityToChar(const std::vector & probabilities) +{ + std::vector occupancies; + for (auto & probability : probabilities) { + occupancies.push_back(convertProbabilityToChar(probability)); + } + return occupancies; +} +// convert [0, 255] to [0, 1] vectors +std::vector convertCharToProbability(const std::vector & occupancies) +{ + std::vector probabilities; + for (auto & occupancy : occupancies) { + probabilities.push_back(convertCharToProbability(occupancy)); + } + return probabilities; +} + +/// @brief fusion with overwrite policy +namespace overwrite_fusion +{ + +/** + * @brief convert char value to occupancy state + * + * @param occupancy [0, 255] + * @return State + */ +State getApproximateState(const unsigned char & occupancy) +{ + if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + return State::OCCUPIED; + } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + return State::FREE; + } else { + return State::UNKNOWN; + } +} + +/** + * @brief override fusion + * + * @param occupancies : occupancies to be fused + * @return unsigned char + */ +unsigned char overwriteFusion(const std::vector & occupancies) +{ + if (occupancies.size() == 0) { + throw std::runtime_error("occupancies size is 0"); + } else if (occupancies.size() == 1) { + return occupancies[0]; + } + + auto fusion_occupancy = 128; // unknown + auto fusion_state = getApproximateState(fusion_occupancy); + for (auto & cell : occupancies) { + auto state = getApproximateState(cell); + if (state > fusion_state) { + fusion_state = state; + fusion_occupancy = cell; + } else if (state < fusion_state) { + continue; + } else { + fusion_occupancy = (fusion_occupancy / 2 + cell / 2); + } + } + return fusion_occupancy; +} +} // namespace overwrite_fusion + +/// @brief fusion with log-odds policy +namespace log_odds_fusion +{ +/** + * @brief log-odds fusion + * + * @param probabilities : probabilities of occupancy [0, 1] + * @return double + */ +double logOddsFusion(const std::vector & probabilities) +{ + double log_odds = 0.0; + for (auto & probability : probabilities) { + const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probability)); + log_odds += std::log(p / (1.0 - p)); + } + return 1.0 / (1.0 + std::exp(-log_odds)); +} + +/** + * @brief weighted log-odds fusion + * + * @param probabilities : probabilities of occupancy [0, 1] + * @param weights : weights of probabilities + * @return double + */ +double logOddsFusion(const std::vector & probabilities, const std::vector & weights) +{ + // check if the size of probabilities and weights are the same + if (probabilities.size() != weights.size()) { + // warning and return normal log-odds fusion + std::cout + << "The size of probabilities and weights are not the same. Return normal log-odds fusion." + << std::endl; + return logOddsFusion(probabilities); + } + + double log_odds = 0.0; + for (size_t i = 0; i < probabilities.size(); i++) { + const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probabilities[i])); + log_odds += weights[i] * std::log(p / (1.0 - p)); + } + return 1.0 / (1.0 + std::exp(-log_odds)); +} +} // namespace log_odds_fusion + +/// @brief fusion with Dempster-Shafer Theory +namespace dempster_shafer_fusion +{ +/** + * @brief conflict modified Dempster-Shafer Theory Data structure + * see https://www.diva-portal.org/smash/get/diva2:852457/FULLTEXT01.pdf + * + */ +struct dempsterShaferOccupancy +{ + double occupied; + double empty; + double unknown; + double conflict_threshold = 0.9; + + // initialize without args + dempsterShaferOccupancy() + { + occupied = 0.0; + empty = 0.0; + unknown = 1.0; + } + + // initialize with probability + dempsterShaferOccupancy(double occupied_probability) + { + // confine to [0, 1] + double p = std::max(0.0, std::min(1.0, occupied_probability)); + occupied = p; + empty = 1.0 - p; + unknown = 0.0; + } + + // initialize with probability and reliability + dempsterShaferOccupancy(double occupied_probability, double reliability) + { + // confine to [0, 1] + double p = std::max(0.0, std::min(1.0, occupied_probability)); + occupied = p * reliability; + empty = (1.0 - p) * reliability; + unknown = 1.0 - occupied - empty; + } + + // normalize + void normalize() + { + double sum = occupied + empty + unknown; + occupied /= sum; + empty /= sum; + unknown /= sum; + } + + // calc conflict factor K + double calcK(const dempsterShaferOccupancy & other) + { + return (occupied * other.empty + empty * other.occupied); + } + // calc sum of occupied probability mass + double calcOccupied(const dempsterShaferOccupancy & other) + { + return occupied * other.occupied + occupied * other.unknown + unknown * other.occupied; + } + // calc sum of empty probability mass + double calcEmpty(const dempsterShaferOccupancy & other) + { + return empty * other.empty + empty * other.unknown + unknown * other.empty; + } + + // Dempster-Shafer fusion + dempsterShaferOccupancy operator+(const dempsterShaferOccupancy & other) + { + dempsterShaferOccupancy result; + double K = calcK(other); + double O = calcOccupied(other); + double E = calcEmpty(other); + + if (K > conflict_threshold) { + // highly conflict + result.occupied = O; + result.empty = E; + result.unknown = 1 - O - E; + } else { + // low conflict + result.occupied = O / (1.0 - K); + result.empty = E / (1.0 - K); + result.unknown = 1 - result.occupied - result.empty; + } + return result; + } + + // get occupancy probability via Pignistic Probability + double getPignisticProbability() { return occupied + unknown / 2.0; } +}; + +/** + * @brief Dempster-Shafer fusion + * + * @param probability + * @return double + */ +double dempsterShaferFusion(const std::vector & probability) +{ + dempsterShaferOccupancy result; // init with unknown + for (auto & p : probability) { + result = result + dempsterShaferOccupancy(p); + } + return result.getPignisticProbability(); +} + +/** + * @brief Dempster-Shafer fusion with reliability + * + * @param probability + * @param reliability + * @return double + */ +double dempsterShaferFusion( + const std::vector & probability, const std::vector & reliability) +{ + // check if the size of probabilities and weights are the same + if (probability.size() != reliability.size()) { + // warning and return normal dempster-shafer fusion probability + std::cout << "The size of probabilities and reliability are not the same. Return normal " + "dempster-shafer fusion." + << std::endl; + return dempsterShaferFusion(probability); + } + + dempsterShaferOccupancy result; // init with unknown + for (size_t i = 0; i < probability.size(); i++) { + result = result + dempsterShaferOccupancy(probability[i], reliability[i]); + } + return result.getPignisticProbability(); +} +} // namespace dempster_shafer_fusion + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method) +{ + if (method == FusionMethod::OVERWRITE) { + return overwrite_fusion::overwriteFusion(occupancy); + } else if (method == FusionMethod::LOG_ODDS) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(log_odds_fusion::logOddsFusion(probability)); + } else if (method == FusionMethod::DEMPSTER_SHAFER) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(dempster_shafer_fusion::dempsterShaferFusion(probability)); + } else { + std::cout << "Unknown fusion method: " << std::endl; + return 128; + } +} + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method, + const std::vector & reliability) +{ + if (method == FusionMethod::OVERWRITE) { + return overwrite_fusion::overwriteFusion(occupancy); + } else if (method == FusionMethod::LOG_ODDS) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(log_odds_fusion::logOddsFusion(probability, reliability)); + } else if (method == FusionMethod::DEMPSTER_SHAFER) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar( + dempster_shafer_fusion::dempsterShaferFusion(probability, reliability)); + } else { + std::cout << "Unknown fusion method: " << std::endl; + return 128; + } +} + +} // namespace fusion_policy diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp new file mode 100644 index 0000000000000..6738286ae5592 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -0,0 +1,458 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "fusion/synchronized_grid_map_fusion_node.hpp" + +#include "cost_value.hpp" +#include "utils/utils.hpp" + +namespace synchronized_grid_map_fusion +{ +using costmap_2d::OccupancyGridMapFixedBlindSpot; +using costmap_2d::OccupancyGridMapLOBFUpdater; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::OccupancyGrid; + +GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) +: Node("synchronized_occupancy_grid_map_fusion", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + /* load input parameters */ + { + // get input topics + declare_parameter("fusion_input_ogm_topics", std::vector{}); + input_topics_ = get_parameter("fusion_input_ogm_topics").as_string_array(); + if (input_topics_.empty()) { + throw std::runtime_error("The number of input topics must larger than 0."); + } + num_input_topics_ = input_topics_.size(); + if (num_input_topics_ < 1) { + RCLCPP_WARN( + this->get_logger(), "minimum num_input_topics_ is 1. current num_input_topics_ is %zu", + num_input_topics_); + num_input_topics_ = 1; + } + if (num_input_topics_ > 12) { + RCLCPP_WARN( + this->get_logger(), "maximum num_input_topics_ is 12. current num_input_topics_ is %zu", + num_input_topics_); + num_input_topics_ = 12; + } + // get input topic reliabilities + declare_parameter("input_ogm_reliabilities", std::vector{}); + input_topic_weights_ = get_parameter("input_ogm_reliabilities").as_double_array(); + + if (input_topic_weights_.empty()) { // no topic weight is set + // set equal weight + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_topic_weights_map_[input_topics_.at(topic_i)] = 1.0 / num_input_topics_; + } + } else if (num_input_topics_ == input_topic_weights_.size()) { + // set weight to map + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_topic_weights_map_[input_topics_.at(topic_i)] = input_topic_weights_.at(topic_i); + } + } else { + throw std::runtime_error("The number of weights does not match the number of topics."); + } + } + + // Set fusion timing parameters + match_threshold_sec_ = declare_parameter("match_threshold_sec", 0.01); + timeout_sec_ = declare_parameter("timeout_sec", 0.1); + input_offset_sec_ = declare_parameter("input_offset_sec", std::vector{}); + if (!input_offset_sec_.empty() && num_input_topics_ != input_offset_sec_.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } else if (input_offset_sec_.empty()) { + // if there are not input offset, set 0.0 + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_offset_sec_.push_back(0.0); + } + } + // Set fusion map parameters + map_frame_ = declare_parameter("map_frame_", "map"); + base_link_frame_ = declare_parameter("base_link_frame_", "base_link"); + gridmap_origin_frame_ = declare_parameter("grid_map_origin_frame_", "base_link"); + fusion_map_length_x_ = declare_parameter("fusion_map_length_x", 100.0); + fusion_map_length_y_ = declare_parameter("fusion_map_length_y", 100.0); + fusion_map_resolution_ = declare_parameter("fusion_map_resolution", 0.5); + + // set fusion method + std::string fusion_method_str = declare_parameter("fusion_method", "overwrite"); + if (fusion_method_str == "overwrite") { + fusion_method_ = fusion_policy::FusionMethod::OVERWRITE; + } else if (fusion_method_str == "log-odds") { + fusion_method_ = fusion_policy::FusionMethod::LOG_ODDS; + } else if (fusion_method_str == "dempster-shafer") { + fusion_method_ = fusion_policy::FusionMethod::DEMPSTER_SHAFER; + } else { + throw std::runtime_error("The fusion method is not supported."); + } + + // sub grid_map + grid_map_subs_.resize(num_input_topics_); + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + std::function fnc = std::bind( + &GridMapFusionNode::onGridMap, this, std::placeholders::_1, input_topics_.at(topic_i)); + grid_map_subs_.at(topic_i) = this->create_subscription( + input_topics_.at(topic_i), rclcpp::QoS{1}.best_effort(), fnc); + } + + // pub grid_map + fused_map_pub_ = this->create_publisher( + "~/output/occupancy_grid_map", rclcpp::QoS{1}.reliable()); + single_frame_pub_ = this->create_publisher( + "~/debug/single_frame_map", rclcpp::QoS{1}.reliable()); + + // updater + occupancy_grid_map_updater_ptr_ = std::make_shared( + fusion_map_length_x_ / fusion_map_resolution_, fusion_map_length_y_ / fusion_map_resolution_, + fusion_map_resolution_); + + // Set timer + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&GridMapFusionNode::timer_callback, this)); + + // set offset map + for (size_t i = 0; i < input_offset_sec_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_sec_[i]; + } + + // init dict + for (size_t i = 0; i < input_topics_.size(); ++i) { + gridmap_dict_[input_topics_[i]] = nullptr; + gridmap_dict_tmp_[input_topics_[i]] = nullptr; + } + + // debug tools + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, "synchronized_grid_map_fusion"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } +} + +/** + * @brief Set the period of the timer + * + * @param new_period + */ +void GridMapFusionNode::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +/** + * @brief Callback function for the grid map message add data to buffer + * + * @param occupancy_grid_msg + * @param topic_name + */ +void GridMapFusionNode::onGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, + const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + const bool is_already_subscribed_this = (gridmap_dict_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + // already subscribed + if (is_already_subscribed_this) { + gridmap_dict_tmp_[topic_name] = occupancy_grid_msg; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + // first time to subscribe in this period + } else { + gridmap_dict_[topic_name] = occupancy_grid_msg; // add to buffer + + // check if all topics are subscribed + bool is_subscribed_all = std::all_of( + std::begin(gridmap_dict_), std::end(gridmap_dict_), + [](const auto & e) { return e.second != nullptr; }); + // Then, go to publish + if (is_subscribed_all) { + for (const auto & e : gridmap_dict_tmp_) { + if (e.second != nullptr) { + gridmap_dict_[e.first] = e.second; + } + } + std::for_each(std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + // if not all topics are subscribed, set timer + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +/** + * @brief Timer callback function + * + */ +void GridMapFusionNode::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +/** + * @brief Publisher function + * + */ +void GridMapFusionNode::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + builtin_interfaces::msg::Time latest_stamp = get_clock()->now(); + double height = 0.0; + + // merge available gridmap + std::vector subscribed_maps; + std::vector weights; + for (const auto & e : gridmap_dict_) { + if (e.second != nullptr) { + subscribed_maps.push_back(GridMapFusionNode::OccupancyGridMsgToGridMap(*e.second)); + weights.push_back(input_topic_weights_map_[e.first]); + latest_stamp = e.second->header.stamp; + height = e.second->info.origin.position.z; + } + } + + // return if empty + if (subscribed_maps.empty()) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "No grid map is subscribed. Please check the topic name and the topic type."); + return; + } + + // fusion map + // single frame gridmap fusion + auto fused_map = SingleFrameOccupancyFusion(subscribed_maps, latest_stamp, weights); + // multi frame gridmap fusion + occupancy_grid_map_updater_ptr_->update(fused_map); + + // publish + fused_map_pub_->publish( + OccupancyGridMapToMsgPtr(map_frame_, latest_stamp, height, *occupancy_grid_map_updater_ptr_)); + single_frame_pub_->publish(OccupancyGridMapToMsgPtr(map_frame_, latest_stamp, height, fused_map)); + + // copy 2nd temp to temp buffer + gridmap_dict_ = gridmap_dict_tmp_; + std::for_each(std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), [](auto & e) { + e.second = nullptr; + }); + + // add processing time for debug + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( + std::vector & occupancy_grid_maps, + const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights) +{ + // if only single map + if (occupancy_grid_maps.size() == 1) { + return occupancy_grid_maps[0]; + } + + // get map to gridmap_origin_frame_ transform + Pose gridmap_origin{}; + try { + gridmap_origin = utils::getPose(latest_stamp, tf_buffer_, gridmap_origin_frame_, map_frame_); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(get_logger(), ex.what()); + return occupancy_grid_maps[0]; + } + + // init fused map with calculated origin + OccupancyGridMapFixedBlindSpot fused_map( + occupancy_grid_maps[0].getSizeInCellsX(), occupancy_grid_maps[0].getSizeInCellsY(), + occupancy_grid_maps[0].getResolution()); + fused_map.updateOrigin( + gridmap_origin.position.x - fused_map.getSizeInMetersX() / 2, + gridmap_origin.position.y - fused_map.getSizeInMetersY() / 2); + + // fix origin of each map + for (auto & map : occupancy_grid_maps) { + map.updateOrigin(fused_map.getOriginX(), fused_map.getOriginY()); + } + + // assume map is same size and resolutions + for (unsigned int x = 0; x < fused_map.getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < fused_map.getSizeInCellsY(); y++) { + // get cost of each map + std::vector costs; + for (auto & map : occupancy_grid_maps) { + costs.push_back(map.getCost(x, y)); + } + + // set fusion policy + auto fused_cost = fusion_policy::singleFrameOccupancyFusion(costs, fusion_method_, weights); + + // set max cost to fused map + fused_map.setCost(x, y, fused_cost); + } + } + + return fused_map; +} + +/** + * @brief Update occupancy grid map with asynchronous input (currently unused) + * + * @param occupancy_grid_msg + */ +void GridMapFusionNode::updateGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg) +{ + // get updater map origin + + // origin is set to current updater map + auto map_for_update = OccupancyGridMsgToCostmap2D(*occupancy_grid_msg); + + // update + occupancy_grid_map_updater_ptr_->update(map_for_update); +} + +nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) +{ + nav2_costmap_2d::Costmap2D costmap2d( + occupancy_grid_map.info.width, occupancy_grid_map.info.height, + occupancy_grid_map.info.resolution, occupancy_grid_map.info.origin.position.x, + occupancy_grid_map.info.origin.position.y, 0); + + for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { + for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { + const unsigned int index = i + j * occupancy_grid_map.info.width; + costmap2d.setCost( + i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + } + } + + return costmap2d; +} + +OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) +{ + OccupancyGridMapFixedBlindSpot gridmap( + occupancy_grid_map.info.width, occupancy_grid_map.info.height, + occupancy_grid_map.info.resolution); + gridmap.updateOrigin( + occupancy_grid_map.info.origin.position.x, occupancy_grid_map.info.origin.position.y); + + for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { + for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { + const unsigned int index = i + j * occupancy_grid_map.info.width; + gridmap.setCost( + i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + } + } + return gridmap; +} + +nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsgPtr( + const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map) +{ + auto msg_ptr = std::make_unique(); + + msg_ptr->header.frame_id = frame_id; + msg_ptr->header.stamp = stamp; + msg_ptr->info.resolution = occupancy_grid_map.getResolution(); + + msg_ptr->info.width = occupancy_grid_map.getSizeInCellsX(); + msg_ptr->info.height = occupancy_grid_map.getSizeInCellsY(); + + double wx{}; + double wy{}; + occupancy_grid_map.mapToWorld(0, 0, wx, wy); + msg_ptr->info.origin.position.x = occupancy_grid_map.getOriginX(); + msg_ptr->info.origin.position.y = occupancy_grid_map.getOriginY(); + msg_ptr->info.origin.position.z = robot_pose_z; + msg_ptr->info.origin.orientation.w = 1.0; + + msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); + + unsigned char * data = occupancy_grid_map.getCharMap(); + for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { + msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + } + return msg_ptr; +} + +} // namespace synchronized_grid_map_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp new file mode 100644 index 0000000000000..e45f3d0686fbc --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp @@ -0,0 +1,69 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" + +#include "cost_value.hpp" + +#include + +namespace costmap_2d +{ + +void OccupancyGridMapLOBFUpdater::initRosParam(rclcpp::Node & /*node*/) +{ + // nothing to load +} + +inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( + const unsigned char & z, const unsigned char & o) +{ + using fusion_policy::convertCharToProbability; + using fusion_policy::convertProbabilityToChar; + using fusion_policy::log_odds_fusion::logOddsFusion; + + constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown_margin = 1; + /* Tau and ST decides how fast the observation decay to the unknown status*/ + constexpr double tau = 0.75; + constexpr double sample_time = 0.1; + + // if the observation is unknown, decay the estimation + if (z >= unknown - unknown_margin && z <= unknown + unknown_margin) { + char diff = static_cast(o) - static_cast(unknown); + const double decay = std::exp(-sample_time / tau); + const double fused = static_cast(unknown) + static_cast(diff) * decay; + return static_cast(fused); + } else { + // else, do the log-odds fusion + const std::vector probability = { + convertCharToProbability(z), convertCharToProbability(o)}; + const unsigned char fused = convertProbabilityToChar(logOddsFusion(probability)); + return fused; + } +} + +bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupancy_grid_map) +{ + updateOrigin( + single_frame_occupancy_grid_map.getOriginX(), single_frame_occupancy_grid_map.getOriginY()); + for (unsigned int x = 0; x < getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < getSizeInCellsY(); y++) { + unsigned int index = getIndex(x, y); + costmap_[index] = applyLOBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + } + } + return true; +} +} // namespace costmap_2d diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 0060754cd875c..b876004d89b0a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -182,4 +182,20 @@ bool extractCommonPointCloud( return true; } +/** + * @brief Convert unsigned char value to closest cost value + * @param cost Cost value + * @return Probability + */ +unsigned char getApproximateOccupancyState(const unsigned char & value) +{ + if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + return occupancy_cost_value::LETHAL_OBSTACLE; + } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { + return occupancy_cost_value::FREE_SPACE; + } else { + return occupancy_cost_value::NO_INFORMATION; + } +} + } // namespace utils diff --git a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md new file mode 100644 index 0000000000000..e1046fa24719d --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md @@ -0,0 +1,168 @@ +# synchronized OGM fusion + +> For simplicity, we use OGM as the meaning of the occupancy grid map. + +This package is used to fuse the OGMs from synchronized sensors. Especially for the lidar. + +Here shows the example OGM for the this synchronized OGM fusion. + +| left lidar OGM | right lidar OGM | top lidar OGM | +| --------------------------------- | ----------------------------------- | ------------------------------- | +| ![left](image/left_lidar_ogm.png) | ![right](image/right_lidar_ogm.png) | ![top](image/top_lidar_ogm.png) | + +OGM fusion with asynchronous sensor outputs is not suitable for this package. Asynchronous OGM fusion is under construction. + +## Processing flow + +The processing flow of this package is shown in the following figure. + +![data_flow](image/synchronized_grid_map_fusion.drawio.svg) + +- Single Frame Fusion + - Single frame fusion means that the OGMs from synchronized sensors are fused in a certain time frame $t=t_n$. +- Multi Frame Fusion + - In the multi frame fusion process, current fused single frame OGM in $t_n$ is fused with the previous fused single frame OGM in $t_{n-1}$. + +## I/O + +| Input topic name | Type | Description | +| ------------------ | ------------------------------------ | --------------------------------------------------------------------------------- | +| `input_ogm_topics` | list of nav_msgs::msg::OccupancyGrid | List of input topics for Occupancy Grid Maps. This parameter is given in list, so | + +| Output topic name | Type | Description | +| ----------------------------- | ---------------------------- | ----------------------------------------------------------------------------- | +| `~/output/occupancy_grid_map` | nav_msgs::msg::OccupancyGrid | Output topic name of the fused Occupancy Grid Map. | +| `~/debug/single_frame_map` | nav_msgs::msg::OccupancyGrid | (debug topic) Output topic name of the single frame fused Occupancy Grid Map. | + +## Parameters + +Synchronized OGM fusion node parameters are shown in the following table. Main parameters to be considered in the fusion node is shown as bold. + +| Ros param name | Sample value | Description | +| --------------------------- | -------------------- | ------------------------------------------------------------- | +| **input_ogm_topics** | ["topic1", "topic2"] | List of input topics for Occupancy Grid Maps | +| **input_ogm_reliabilities** | [0.8, 0.2] | Weights for the reliability of each input topic | +| **fusion_method** | "overwrite" | Method of fusion ("overwrite", "log-odds", "dempster-shafer") | +| match_threshold_sec | 0.01 | Matching threshold in milliseconds | +| timeout_sec | 0.1 | Timeout duration in seconds | +| input_offset_sec | [0.0, 0.0] | Offset time in seconds for each input topic | +| map*frame* | "map" | Frame name for the fused map | +| base*link_frame* | "base_link" | Frame name for the base link | +| grid*map_origin_frame* | "base_link" | Frame name for the origin of the grid map | +| fusion_map_length_x | 100.0 | Length of the fused map along the X-axis | +| fusion_map_length_y | 100.0 | Length of the fused map along the Y-axis | +| fusion_map_resolution | 0.5 | Resolution of the fused map | + +Since this node assumes that the OGMs from synchronized sensors are generated in the same time, we need to tune the `match_threshold_sec`, `timeout_sec` and `input_offset_sec` parameters to successfully fuse the OGMs. + +## Fusion methods + +For the single frame fusion, the following fusion methods are supported. + +| Fusion Method in parameter | Description | +| -------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `overwrite` | The value of the cell in the fused OGM is overwritten by the value of the cell in the OGM with the highest priority.
We set priority as `Occupied` > `Free` > `Unknown`. | +| `log-odds` | The value of the cell in the fused OGM is calculated by the log-odds ratio method, which is known as a Bayesian fusion method.
The log-odds of a probability $p$ can be written as $l_p = \log(\frac{p}{1-p})$.
And the fused log-odds is calculated by the sum of log-odds. $l_f = \Sigma l_p$ | +| `dempster-shafer` | The value of the cell in the fused OGM is calculated by the Dempster-Shafer theory[1]. This is also popular method to handle multiple evidences. This package applied conflict escape logic in [2] for the performance. See references for the algorithm details. | + +For the multi frame fusion, currently only supporting `log-odds` fusion method. + +## How to use + +### launch fusion node + +The minimum node launch will be like the following. + +```xml + + + + + + + + + + +``` + +### (Optional) Generate OGMs in each sensor frame + +You need to generate OGMs in each sensor frame before achieving grid map fusion. + +`probabilistic_occupancy_grid_map` package supports to generate OGMs for the each from the point cloud data. + +
+ Example launch.xml (click to expand) + +```xml + + + + + + + + + + + + + + + +The minimum parameter for the OGM generation in each frame is shown in the following table. + +|Parameter|Description| +|--|--| +|`input/obstacle_pointcloud`| The input point cloud data for the OGM generation. This point cloud data should be the point cloud data which is segmented as the obstacle.| +|`input/raw_pointcloud`| The input point cloud data for the OGM generation. This point cloud data should be the point cloud data which is not segmented as the obstacle. | +|`output`| The output topic of the OGM. | +|`map_frame`| The tf frame for the OGM center origin. | +|`scan_origin`| The tf frame for the sensor origin. | +|`method`| The method for the OGM generation. Currently we support `pointcloud_based_occupancy_grid_map` and `laser_scan_based_occupancy_grid_map`. The pointcloud based method is recommended. | +|`param_file`| The parameter file for the OGM generation. See [example parameter file](config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml) | + +``` + +
+ +
+ +We recommend to use same `map_frame`, size and resolutions for the OGMs from synchronized sensors. +Also, remember to set `enable_single_frame_mode` and `filter_obstacle_pointcloud_by_raw_pointcloud` to `true` in the `probabilistic_occupancy_grid_map` package (you do not need to set these parameters if you use the above example config file). + +
+ +### Run both OGM generation node and fusion node + +We prepared the launch file to run both OGM generation node and fusion node in [`grid_map_fusion_with_synchronized_pointclouds.launch.py`](launch/grid_map_fusion_with_synchronized_pointclouds.launch.py) + +You can include this launch file like the following. + +```xml + + + + + + + + + + +``` + +The minimum parameter for the launch file is shown in the following table. + +| Parameter | Description | +| -------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `output` | The output topic of the finally fused OGM. | +| `method` | The method for the OGM generation. Currently we support `pointcloud_based_occupancy_grid_map` and `laser_scan_based_occupancy_grid_map`. The pointcloud based method is recommended. | +| `fusion_config_file` | The parameter file for the grid map fusion. See [example parameter file](config/grid_map_fusion.param.yaml) | +| `ogm_config_file` | The parameter file for the OGM generation. See [example parameter file](config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml) | + +## References + +- [1] Dempster, A. P., Laird, N. M., & Rubin, D. B. (1977). Maximum likelihood from incomplete data via the EM algorithm. Journal of the Royal Statistical Society. Series B (Methodological), 39(1), 1-38. +- [2] diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp new file mode 100644 index 0000000000000..baf94fabfd2a1 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "cost_value.hpp" + +#include "gtest/gtest.h" + +// Test the CostTranslationTable and InverseCostTranslationTable functions +using occupancy_cost_value::cost_translation_table; +using occupancy_cost_value::inverse_cost_translation_table; + +TEST(CostTranslationTableTest, TestRange) +{ + // Test the value range of CostTranslationTable + for (int i = 0; i < 256; i++) { + EXPECT_GE(cost_translation_table[i], 1); + EXPECT_LE(cost_translation_table[i], 99); + } + + // Test the value range of InverseCostTranslationTable + for (int i = 1; i < 100; i++) { + EXPECT_GE(inverse_cost_translation_table[i], 0); + EXPECT_LE(inverse_cost_translation_table[i], 255); + } +} + +TEST(CostTranslationTableTest, TestConversion) +{ + // Test the forward and inverse conversion of 0, 128, and 255 + EXPECT_EQ(cost_translation_table[0], 1); + EXPECT_EQ(cost_translation_table[128], 50); + EXPECT_EQ(cost_translation_table[255], 99); + EXPECT_EQ(inverse_cost_translation_table[1], 2); + EXPECT_EQ(inverse_cost_translation_table[50], 128); + EXPECT_EQ(inverse_cost_translation_table[99], 255); +} diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp new file mode 100644 index 0000000000000..2501a361fba58 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "cost_value.hpp" +#include "fusion/single_frame_fusion_policy.hpp" +#include "gtest/gtest.h" + +// Test the log-odds update rule +TEST(FusionPolicyTest, TestLogOddsUpdateRule) +{ + using fusion_policy::log_odds_fusion::logOddsFusion; + const double MARGIN = 0.03; + const double OCCUPIED = 1.0 - MARGIN; + const double FREE = 0.0 + MARGIN; + const double UNKNOWN = 0.5; + const double EPSILON = 1e-3; + + // same case + std::vector case1_1 = {OCCUPIED, OCCUPIED}; + std::vector case1_2 = {FREE, FREE}; + std::vector case1_3 = {UNKNOWN, UNKNOWN}; + EXPECT_GE(logOddsFusion(case1_1), OCCUPIED); + EXPECT_LE(logOddsFusion(case1_2), FREE); + EXPECT_NEAR(logOddsFusion(case1_3), UNKNOWN, EPSILON); + + // with unknown + std::vector case2_1 = {OCCUPIED, UNKNOWN}; + std::vector case2_2 = {FREE, UNKNOWN}; + EXPECT_NEAR(logOddsFusion(case2_1), OCCUPIED, EPSILON); + EXPECT_NEAR(logOddsFusion(case2_2), FREE, EPSILON); + + // with conflict + std::vector case3_1 = {OCCUPIED, FREE}; + EXPECT_NEAR(logOddsFusion(case3_1), UNKNOWN, EPSILON); +} + +// Test the dempster-shafer update rule +TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) +{ + using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + const double MARGIN = 0.03; + const double OCCUPIED = 1.0 - MARGIN; + const double FREE = 0.0 + MARGIN; + const double UNKNOWN = 0.5; + const double EPSILON = 1e-3; + + // same case + std::vector case1_1 = {OCCUPIED, OCCUPIED}; + std::vector case1_2 = {FREE, FREE}; + std::vector case1_3 = {UNKNOWN, UNKNOWN}; + EXPECT_GE(dempsterShaferFusion(case1_1), OCCUPIED); + EXPECT_LE(dempsterShaferFusion(case1_2), FREE); + EXPECT_NEAR(dempsterShaferFusion(case1_3), UNKNOWN, EPSILON); + + // with unknown + std::vector case2_1 = {OCCUPIED, UNKNOWN}; + std::vector case2_2 = {FREE, UNKNOWN}; + EXPECT_NEAR(dempsterShaferFusion(case2_1), OCCUPIED, EPSILON); + EXPECT_NEAR(dempsterShaferFusion(case2_2), FREE, EPSILON); + + // with conflict + std::vector case3_1 = {OCCUPIED, FREE}; + EXPECT_NEAR(dempsterShaferFusion(case3_1), UNKNOWN, EPSILON); +} diff --git a/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py new file mode 100644 index 0000000000000..c15daaf886b6f --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py @@ -0,0 +1,255 @@ +# Copyright 2021 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. + +import time +import unittest + +from geometry_msgs.msg import TransformStamped +import launch +import launch.actions +from launch_ros.substitutions import FindPackageShare +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.markers +import launch_testing.tools +from nav_msgs.msg import OccupancyGrid +import numpy as np +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +PACKAGE_NAME = "probabilistic_occupancy_grid_map" +INPUT_TOPICS = ["/topic1", "/topic2"] +DEBUG_OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/debug/single_frame_map" +OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/output/occupancy_grid_map" + +FREE_VALUE = 1 +UNKNOWN_VALUE = 50 +OCCUPIED_VALUE = 99 + + +# test launcher to launch grid map fusion node +@pytest.mark.launch_test +def generate_test_description(): + """Launch file test description. + + Returns: + _type_: launch.LaunchDescription + """ + # get launch file path + launch_file_path = ( + FindPackageShare(PACKAGE_NAME).find(PACKAGE_NAME) + + "/launch/synchronized_occupancy_grid_map_fusion.launch.xml" + ) + # use default launch arguments and parms + launch_args = [] + # action to include launch file + test_launch_file = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.AnyLaunchDescriptionSource(launch_file_path), + launch_arguments=launch_args, + ) + + return launch.LaunchDescription( + [ + test_launch_file, + launch_testing.actions.ReadyToTest(), + ] + ) + + +# util functions +def create_nav_msgs_occupancy_grid_msg(fill_value: int = 0, stamp: rclpy.time.Time = None): + """Create nav_msgs occupancy grid message. + + Args: + fill_value (int, optional): fill value. Defaults to 0. + + Returns: + OccupancyGrid: nav_msgs occupancy grid message + """ + msg = OccupancyGrid() + msg.header.stamp = rclpy.clock.Clock().now().to_msg() if stamp is None else stamp.to_msg() + msg.header.frame_id = "map" + msg.info.resolution = 0.5 # 0.5m x 0.5m + msg.info.width = 200 # 100m x 100m + msg.info.height = 200 + msg.info.origin.position.x = -msg.info.width * msg.info.resolution / 2.0 + msg.info.origin.position.y = -msg.info.height * msg.info.resolution / 2.0 + msg.info.origin.position.z = 0.0 + msg.info.origin.orientation.x = 0.0 + msg.info.origin.orientation.y = 0.0 + msg.info.origin.orientation.z = 0.0 + msg.info.origin.orientation.w = 1.0 + msg.data = [fill_value] * msg.info.width * msg.info.height + return msg + + +def parse_ogm_msg(msg: OccupancyGrid): + """Parse nav_msgs occupancy grid message. + + Args: + msg (OccupancyGrid): nav_msgs occupancy grid message + + Returns: + np.ndarray: occupancy grid map + """ + ogm = np.array(msg.data).reshape((msg.info.height, msg.info.width)) + return ogm + + +# dummy tf broadcaster +def generate_static_transform_msg(stamp: rclpy.time.Time = None): + """Generate static transform message from base_link to map. + + Returns: + TransformStamped: static transform message + """ + msg = TransformStamped() + if stamp is None: + msg.header.stamp = rclpy.clock.Clock().now().to_msg() + else: + msg.header.stamp = stamp.to_msg() + msg.header.frame_id = "map" + msg.child_frame_id = "base_link" + msg.transform.translation.x = 0.0 + msg.transform.translation.y = 0.0 + msg.transform.translation.z = 0.0 + msg.transform.rotation.x = 0.0 + msg.transform.rotation.y = 0.0 + msg.transform.rotation.z = 0.0 + msg.transform.rotation.w = 1.0 + return msg + + +# --- TestSynchronizedOGMFusion --- +# 1. test free ogm and free ogm input fusion +# 2. test occupied ogm and occupied ogm input fusion +# 3. test unknown ogm and free ogm input fusion +# 4. test unknown ogm and occupied ogm input fusion +# 5. test free ogm and occupied ogm input fusion +class TestSynchronizedOGMFusion(unittest.TestCase): + @classmethod + def setUpClass(cls): + # init ROS at once + rclpy.init() + + @classmethod + def tearDownClass(cls): + # shutdown ROS at once + rclpy.shutdown() + + def setUp(self): + # called when each test started + self.node = rclpy.create_node("grid_map_fusion_node_test_node") + # send static transform from map to base_link + tf_msg = generate_static_transform_msg() + self.tf_broadcaster = StaticTransformBroadcaster(self.node) + self.tf_broadcaster.sendTransform(tf_msg) + + def tearDown(self): + # called when each test finished + self.node.destroy_node() + + def callback(self, msg: OccupancyGrid): + self.msg_buffer.append(msg) + print("callback", len(self.msg_buffer)) + + def get_newest_ogm_value(self): + return np.mean(self.msg_buffer[-1].data) + + # util functions test 1~3 + def create_pub_sub(self): + # create publisher + pub1 = self.node.create_publisher(OccupancyGrid, INPUT_TOPICS[0], 10) + pub2 = self.node.create_publisher(OccupancyGrid, INPUT_TOPICS[1], 10) + + # create subscriber for debug output + sensor_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.msg_buffer = [] + # subscribe to occupancy grid with buffer + sub = self.node.create_subscription( + OccupancyGrid, DEBUG_OUTPUT_TOPIC, self.callback, qos_profile=sensor_qos + ) + return [pub1, pub2], sub + + # test functions + def test_same_ogm_fusion(self): + """Test 1~3. + + Expected output: same ogm. + """ + # wait for the node to be ready + time.sleep(3) + pubs, sub = self.create_pub_sub() # pubs have two publishers + + fill_values = [FREE_VALUE, OCCUPIED_VALUE, UNKNOWN_VALUE] + + for fill_value in fill_values: + # create free/occupied/unknown ogm + ogm = create_nav_msgs_occupancy_grid_msg(fill_value=fill_value) + # publish free/occupied/unknown ogm + pubs[0].publish(ogm) + pubs[1].publish(ogm) + + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=2.0) + fused_mean = self.get_newest_ogm_value() + print("same ogm test: ", fill_value, fused_mean) + # assert almost equal + self.assertAlmostEqual(fused_mean, fill_value, delta=3.0) + + def test_unknown_fusion(self): + """Test unknown ogm and free ogm input fusion. + + Expected output: free, occupied, unknown. + """ + # wait for the node to be ready + time.sleep(3) + pubs, sub = self.create_pub_sub() + + fill_values = [FREE_VALUE, OCCUPIED_VALUE, UNKNOWN_VALUE] + now = rclpy.clock.Clock().now() + unknown_ogm = create_nav_msgs_occupancy_grid_msg(fill_value=UNKNOWN_VALUE, stamp=now) + + for fill_value in fill_values: + # publish unknown ogm + pubs[0].publish(unknown_ogm) + # create free/occupied/unknown ogm + ogm = create_nav_msgs_occupancy_grid_msg(fill_value=fill_value, stamp=now) + # publish ogm + pubs[1].publish(ogm) + + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=2.0) + fused_mean = self.get_newest_ogm_value() + print("unknown ogm test: ", fill_value, fused_mean) + # assert almost equal + self.assertAlmostEqual(fused_mean, fill_value, delta=3.0) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # check exit code + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 3a00bb013d867..1ebc14c4e6f93 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "traffic_light_classifier/nodelet.hpp" +#include + #include #include #include @@ -98,16 +100,18 @@ void TrafficLightClassifierNodelet::imageRoiCallback( output_msg.signals.resize(input_rois_msg->rois.size()); std::vector images; - size_t num_valid_roi = 0; std::vector backlight_indices; for (size_t i = 0; i < input_rois_msg->rois.size(); i++) { - // skip if not the expected type of roi + // skip if the roi is not detected + if (input_rois_msg->rois.at(i).roi.height == 0) { + break; + } if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) { continue; } - output_msg.signals[num_valid_roi].traffic_light_id = + output_msg.signals[images.size()].traffic_light_id = input_rois_msg->rois.at(i).traffic_light_id; - output_msg.signals[num_valid_roi].traffic_light_type = + output_msg.signals[images.size()].traffic_light_type = input_rois_msg->rois.at(i).traffic_light_type; const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; @@ -116,15 +120,29 @@ void TrafficLightClassifierNodelet::imageRoiCallback( backlight_indices.emplace_back(i); } images.emplace_back(roi_img); - num_valid_roi++; } - output_msg.signals.resize(num_valid_roi); + output_msg.signals.resize(images.size()); if (!classifier_ptr_->getTrafficSignals(images, output_msg)) { RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); return; } + // append the undetected rois as unknown + for (const auto & input_roi : input_rois_msg->rois) { + if (input_roi.roi.height == 0 && input_roi.traffic_light_type == classify_traffic_light_type_) { + tier4_perception_msgs::msg::TrafficSignal tlr_sig; + tlr_sig.traffic_light_id = input_roi.traffic_light_id; + tlr_sig.traffic_light_type = input_roi.traffic_light_type; + tier4_perception_msgs::msg::TrafficLightElement element; + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::CIRCLE; + element.confidence = 0.0; + tlr_sig.elements.push_back(element); + output_msg.signals.push_back(tlr_sig); + } + } + for (const auto & idx : backlight_indices) { auto & elements = output_msg.signals.at(idx).elements; for (auto & element : elements) { diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index 257e9fc7ea99d..8037dc5472fbe 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -272,14 +272,28 @@ void TrafficLightFineDetectorNodelet::detectionMatch( } out_rois.rois.clear(); - for (const auto & p : bestDetections) { + std::vector invalid_roi_id; + for (const auto & [tlr_id, roi] : id2expectRoi) { + // if matches, update the roi info + if (!bestDetections.count(tlr_id)) { + invalid_roi_id.emplace_back(tlr_id); + continue; + } + TrafficLightRoi tlr; + tlr.traffic_light_id = tlr_id; + const auto & object = bestDetections.at(tlr_id); + tlr.traffic_light_type = roi.traffic_light_type; + tlr.roi.x_offset = object.x_offset; + tlr.roi.y_offset = object.y_offset; + tlr.roi.width = object.width; + tlr.roi.height = object.height; + out_rois.rois.push_back(tlr); + } + // append undetected rois at the end + for (const auto & id : invalid_roi_id) { TrafficLightRoi tlr; - tlr.traffic_light_id = p.first; - tlr.traffic_light_type = id2expectRoi[p.first].traffic_light_type; - tlr.roi.x_offset = p.second.x_offset; - tlr.roi.y_offset = p.second.y_offset; - tlr.roi.width = p.second.width; - tlr.roi.height = p.second.height; + tlr.traffic_light_id = id; + tlr.traffic_light_type = id2expectRoi[id].traffic_light_type; out_rois.rois.push_back(tlr); } } diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 97fa98ab18a72..6429640ff44c1 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -129,12 +129,18 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( const uint8_t traffic_light_type) { std::vector occlusion_ratios; + size_t not_detected_roi = 0; if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) { occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg; selected_roi_msg.rois.reserve(in_roi_msg->rois.size()); for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) { + // not detected roi + if (in_roi_msg->rois[i].roi.height == 0) { + not_detected_roi++; + continue; + } if (in_roi_msg->rois.at(i).traffic_light_type == traffic_light_type) { selected_roi_msg.rois.push_back(in_roi_msg->rois.at(i)); } @@ -142,7 +148,7 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; - if (selected_roi_msg.rois.size() != in_signal_msg->signals.size()) { + if (selected_roi_msg.rois.size() != in_signal_msg->signals.size() - not_detected_roi) { occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { auto selected_roi_msg_ptr = @@ -162,6 +168,12 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( traffic_light_utils::setSignalUnknown(out_msg_.signals.at(predicted_num + i), 0.0); } } + + // push back not detected rois + for (size_t i = occlusion_ratios.size(); i < in_signal_msg->signals.size(); ++i) { + out_msg_.signals.push_back(in_signal_msg->signals[i]); + } + subscribed_.at(traffic_light_type) = true; if (std::all_of(subscribed_.begin(), subscribed_.end(), [](bool v) { return v; })) { diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 7daa0849abe5e..6f96913420bad 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -88,6 +88,10 @@ void CloudOcclusionPredictor::predict( image_geometry::PinholeCameraModel pinhole_model; pinhole_model.fromCameraInfo(*camera_info_msg); for (size_t i = 0; i < rois_msg->rois.size(); i++) { + // skip if no detection + if (rois_msg->rois[i].roi.height == 0) { + continue; + } calcRoiVector3D( rois_msg->rois[i], pinhole_model, traffic_light_position_map, tf_camera2map, roi_tls[i], roi_brs[i]); @@ -107,7 +111,7 @@ void CloudOcclusionPredictor::predict( lidar_rays_[static_cast(ray.azimuth)][static_cast(ray.elevation)].push_back(ray); } for (size_t i = 0; i < roi_tls.size(); i++) { - occlusion_ratios[i] = predict(roi_tls[i], roi_brs[i]); + occlusion_ratios[i] = rois_msg->rois[i].roi.height == 0 ? 0 : predict(roi_tls[i], roi_brs[i]); } } diff --git a/planning/.pages b/planning/.pages index c41a1cc8c6603..e244f1d1ea25d 100644 --- a/planning/.pages +++ b/planning/.pages @@ -12,7 +12,7 @@ nav: - 'Scene Module': - 'Avoidance': planning/behavior_path_avoidance_module - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module - - 'Dynamic Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design + - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module - 'Goal Planner': planning/behavior_path_goal_planner_module - 'Lane Change': planning/behavior_path_lane_change_module - 'Side Shift': planning/behavior_path_side_shift_module diff --git a/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt b/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt new file mode 100644 index 0000000000000..98874b8923324 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_dynamic_avoidance_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/manager.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_dynamic_avoidance_module/README.md similarity index 94% rename from planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md rename to planning/behavior_path_dynamic_avoidance_module/README.md index f303937bd0f78..5e8be9136886c 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md +++ b/planning/behavior_path_dynamic_avoidance_module/README.md @@ -32,16 +32,16 @@ First, a maximum lateral offset to avoid is calculated as follows. The polygon's width to extract from the drivable area is the obstacle width and double `drivable_area_generation.lat_offset_from_obstacle`. We can limit the lateral shift offset by `drivable_area_generation.max_lat_offset_to_avoid`. -![drivable_area_extraction_width](../image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg) +![drivable_area_extraction_width](./image/drivable_area_extraction_width.drawio.svg) Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision). Regarding the same directional obstacles, obstacles whose TTC is negative will be ignored (e.g. The obstacle is in front of the ego, and the obstacle's velocity is larger than the ego's velocity.). Same directional obstacles -![same_directional_object](../image/dynamic_avoidance/same_directional_object.svg) +![same_directional_object](./image/same_directional_object.svg) Opposite directional obstacles -![opposite_directional_object](../image/dynamic_avoidance/opposite_directional_object.svg) +![opposite_directional_object](./image/opposite_directional_object.svg) ## Parameters diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml similarity index 95% rename from planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml rename to planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml index 29fdca78e5bcb..371f7da2fadf5 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml @@ -59,6 +59,10 @@ max_time_for_object_lat_shift: 0.0 # [s] lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] + max_ego_lat_acc: 0.3 # [m/ss] + max_ego_lat_jerk: 0.3 # [m/sss] + delay_time_ego_shift: 1.0 # [s] + # for same directional object overtaking_object: max_time_to_collision: 40.0 # [s] diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg b/planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg rename to planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg b/planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg rename to planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg b/planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg rename to planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp rename to planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp index 9e08f309a78b3..c2c17c4e55c1e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_dynamic_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -52,4 +52,4 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp rename to planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index 3ceff4244f2c8..d2f9cd95066d9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" @@ -23,9 +23,6 @@ #include #include #include -#include -#include -#include #include #include @@ -35,6 +32,25 @@ #include #include +namespace +{ +template +bool isInVector(const T & val, const std::vector & vec) +{ + return std::find(vec.begin(), vec.end(), val) != vec.end(); +} + +template +std::vector getAllKeys(const std::unordered_map & map) +{ + std::vector keys; + for (const auto & pair : map) { + keys.push_back(pair.first); + } + return keys; +} +} // namespace + namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedPath; @@ -97,6 +113,10 @@ struct DynamicAvoidanceParameters double max_time_for_lat_shift{0.0}; double lpf_gain_for_lat_avoid_to_offset{0.0}; + double max_ego_lat_acc{0.0}; + double max_ego_lat_jerk{0.0}; + double delay_time_ego_shift{0.0}; + double max_time_to_collision_overtaking_object{0.0}; double start_duration_to_avoid_overtaking_object{0.0}; double end_duration_to_avoid_overtaking_object{0.0}; @@ -113,6 +133,11 @@ struct TimeWhileCollision double time_to_end_collision; }; +struct LatFeasiblePaths +{ + std::vector left_path; + std::vector right_path; +}; class DynamicAvoidanceModule : public SceneModuleInterface { public: @@ -151,6 +176,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool is_collision_left{false}; bool should_be_avoided{false}; std::vector ref_path_points_for_obj_poly; + LatFeasiblePaths ego_lat_feasible_paths; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, @@ -172,14 +198,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface { } int max_count_{0}; - int min_count_{0}; + int min_count_{0}; // TODO(murooka): The sign needs to be opposite? void initialize() { current_uuids_.clear(); } void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) { // add/update object if (object_map_.count(uuid) != 0) { + const auto prev_object = object_map_.at(uuid); object_map_.at(uuid) = object; + // TODO(murooka) refactor this. Counter can be moved to DynamicObject, + // and TargetObjectsManager can be removed. + object_map_.at(uuid).ego_lat_feasible_paths = prev_object.ego_lat_feasible_paths; } else { object_map_.emplace(uuid, object); } @@ -195,14 +225,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface current_uuids_.push_back(uuid); } - void finalize() + void finalize(const LatFeasiblePaths & ego_lat_feasible_paths) { // decrease counter for not updated uuids std::vector not_updated_uuids; for (const auto & object : object_map_) { - if ( - std::find(current_uuids_.begin(), current_uuids_.end(), object.first) == - current_uuids_.end()) { + if (!isInVector(object.first, current_uuids_)) { not_updated_uuids.push_back(object.first); } } @@ -214,45 +242,46 @@ class DynamicAvoidanceModule : public SceneModuleInterface } } - // remove objects whose counter is lower than threshold - std::vector obsolete_uuids; + // update valid object uuids and its variable for (const auto & counter : counter_map_) { - if (counter.second < min_count_) { - obsolete_uuids.push_back(counter.first); + if (!isInVector(counter.first, valid_object_uuids_) && max_count_ <= counter.second) { + valid_object_uuids_.push_back(counter.first); + object_map_.at(counter.first).ego_lat_feasible_paths = ego_lat_feasible_paths; } } - for (const auto & obsolete_uuid : obsolete_uuids) { - counter_map_.erase(obsolete_uuid); - object_map_.erase(obsolete_uuid); + valid_object_uuids_.erase( + std::remove_if( + valid_object_uuids_.begin(), valid_object_uuids_.end(), + [&](const auto & uuid) { + return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < max_count_; + }), + valid_object_uuids_.end()); + + // remove objects whose counter is lower than threshold + const auto counter_map_keys = getAllKeys(counter_map_); + for (const auto & key : counter_map_keys) { + if (counter_map_.at(key) < min_count_) { + counter_map_.erase(key); + object_map_.erase(key); + } } } std::vector getValidObjects() const { - std::vector objects; - for (const auto & object : object_map_) { - if (counter_map_.count(object.first) != 0) { - if (max_count_ <= counter_map_.at(object.first)) { - objects.push_back(object.second); - } + std::vector valid_objects; + for (const auto & valid_object_uuid : valid_object_uuids_) { + if (object_map_.count(valid_object_uuid) == 0) { + std::cerr + << "[DynamicAvoidance] Internal calculation has an error when getting valid objects." + << std::endl; + continue; } + valid_objects.push_back(object_map_.at(valid_object_uuid)); } - return objects; - } - std::optional getValidObject(const std::string & uuid) const - { - // add/update object - if (counter_map_.count(uuid) == 0) { - return std::nullopt; - } - if (counter_map_.at(uuid) < max_count_) { - return std::nullopt; - } - if (object_map_.count(uuid) == 0) { - return std::nullopt; - } - return object_map_.at(uuid); + + return valid_objects; } - void updateObject( + void updateObjectVariables( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, const bool should_be_avoided, @@ -269,6 +298,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface // NOTE: positive is for meeting entry condition, and negative is for exiting. std::unordered_map counter_map_; std::unordered_map object_map_; + std::vector valid_object_uuids_{}; }; struct DecisionWithReason @@ -319,6 +349,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); + LatFeasiblePaths generateLateralFeasiblePaths( + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const; void updateRefPathBeforeLaneChange(const std::vector & ego_ref_path_points); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, @@ -378,4 +410,4 @@ class DynamicAvoidanceModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_dynamic_avoidance_module/package.xml b/planning/behavior_path_dynamic_avoidance_module/package.xml new file mode 100644 index 0000000000000..11792a15b2cd2 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/package.xml @@ -0,0 +1,42 @@ + + + + behavior_path_dynamic_avoidance_module + 0.1.0 + The behavior_path_dynamic_avoidance_module package + + Takayuki Murooka + Yuki Takagi + Satoshi Ota + Kosuke Takeuchi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + lanelet2_core + lanelet2_extension + motion_utils + object_recognition_utils + pluginlib + rclcpp + signal_processing + tier4_autoware_utils + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_dynamic_avoidance_module/plugins.xml similarity index 72% rename from planning/behavior_path_planner/plugins.xml rename to planning/behavior_path_dynamic_avoidance_module/plugins.xml index a16f3dd7bddee..fd2e1bc4137b7 100644 --- a/planning/behavior_path_planner/plugins.xml +++ b/planning/behavior_path_dynamic_avoidance_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp similarity index 95% rename from planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp rename to planning/behavior_path_dynamic_avoidance_module/src/manager.cpp index 73d263319c2d9..569c6a17b5b32 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "behavior_path_dynamic_avoidance_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -117,6 +117,10 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) p.lpf_gain_for_lat_avoid_to_offset = node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); + p.max_ego_lat_acc = node->declare_parameter(ns + "max_ego_lat_acc"); + p.max_ego_lat_jerk = node->declare_parameter(ns + "max_ego_lat_jerk"); + p.delay_time_ego_shift = node->declare_parameter(ns + "delay_time_ego_shift"); + p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); p.start_duration_to_avoid_overtaking_object = @@ -233,6 +237,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); + updateParam(parameters, ns + "max_ego_lat_acc", p->max_ego_lat_acc); + updateParam(parameters, ns + "max_ego_lat_jerk", p->max_ego_lat_jerk); + updateParam(parameters, ns + "delay_time_ego_shift", p->delay_time_ego_shift); + updateParam( parameters, ns + "overtaking_object.max_time_to_collision", p->max_time_to_collision_overtaking_object); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp similarity index 88% rename from planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp rename to planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index 87a940d04e8d3..ca58bbf0adcaa 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_dynamic_avoidance_module/scene.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -262,6 +262,35 @@ double calcDistanceToSegment( first_to_second * first_to_target.dot(first_to_second) / std::pow(first_to_second.norm(), 2); return (Eigen::Vector2d{p1.x, p1.y} - p2_nearest).norm(); } + +std::optional> intersectLines( + const std::vector & source_line, + const std::vector & target_line) +{ + for (int source_seg_idx = 0; source_seg_idx < static_cast(source_line.size()) - 1; + ++source_seg_idx) { + for (int target_seg_idx = 0; target_seg_idx < static_cast(target_line.size()) - 1; + ++target_seg_idx) { + const auto intersect_point = tier4_autoware_utils::intersect( + source_line.at(source_seg_idx).position, source_line.at(source_seg_idx + 1).position, + target_line.at(target_seg_idx), target_line.at(target_seg_idx + 1)); + if (intersect_point) { + return std::make_pair(source_seg_idx, *intersect_point); + } + } + } + return std::nullopt; +} + +std::vector convertToPoints( + const std::vector & poses) +{ + std::vector points; + for (const auto & pose : poses) { + points.push_back(pose.position); + } + return points; +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( @@ -310,6 +339,9 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { + info_marker_.markers.clear(); + debug_marker_.markers.clear(); + // calculate target objects updateTargetObjects(); @@ -329,9 +361,6 @@ bool DynamicAvoidanceModule::canTransitSuccessState() BehaviorModuleOutput DynamicAvoidanceModule::plan() { - info_marker_.markers.clear(); - debug_marker_.markers.clear(); - const auto & input_path = getPreviousModuleOutput().path; // create obstacles to avoid (= extract from the drivable area) @@ -425,6 +454,10 @@ void DynamicAvoidanceModule::updateTargetObjects() updateRefPathBeforeLaneChange(input_ref_path_points); + const auto & ego_pose = getEgoPose(); + const double ego_vel = getEgoSpeed(); + const auto ego_lat_feasible_paths = generateLateralFeasiblePaths(ego_pose, ego_vel); + // 1. Rough filtering of target objects target_objects_manager_.initialize(); for (const auto & predicted_object : predicted_objects) { @@ -509,7 +542,7 @@ void DynamicAvoidanceModule::updateTargetObjects() latest_time_inside_ego_path); target_objects_manager_.updateObject(obj_uuid, target_object); } - target_objects_manager_.finalize(); + target_objects_manager_.finalize(ego_lat_feasible_paths); // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { @@ -580,18 +613,24 @@ void DynamicAvoidanceModule::updateTargetObjects() parameters_->max_time_to_collision_overtaking_object < time_to_collision) || (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { + const auto time_to_collision_str = time_to_collision == std::numeric_limits::max() + ? "infinity" + : std::to_string(time_to_collision); RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", - obj_uuid.c_str(), time_to_collision); + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%s) is large.", + obj_uuid.c_str(), time_to_collision_str.c_str()); continue; } if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + const auto time_to_collision_str = time_to_collision == std::numeric_limits::max() + ? "infinity" + : std::to_string(time_to_collision); RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small " + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%s) is a small " "negative value.", - obj_uuid.c_str(), time_to_collision); + obj_uuid.c_str(), time_to_collision_str.c_str()); continue; } } @@ -611,7 +650,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const double signed_dist_ego_to_obj = [&]() { const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( - input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + input_path.points, ego_pose.position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + @@ -649,7 +688,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } const bool should_be_avoided = true; - target_objects_manager_.updateObject( + target_objects_manager_.updateObjectVariables( obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided, ref_path_points_for_obj_poly); } @@ -657,6 +696,42 @@ void DynamicAvoidanceModule::updateTargetObjects() prev_input_ref_path_points_ = input_ref_path_points; } +LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const +{ + const double lat_acc = parameters_->max_ego_lat_acc; + const double lat_jerk = parameters_->max_ego_lat_jerk; + const double delay_time = parameters_->delay_time_ego_shift; + + LatFeasiblePaths ego_lat_feasible_paths; + for (double t = 0; t < 10.0; t += 1.0) { + const double feasible_lat_offset = lat_acc * std::pow(std::max(t - delay_time, 0.0), 2) / 2.0 + + lat_jerk * std::pow(std::max(t - delay_time, 0.0), 3) / 6.0 - + planner_data_->parameters.vehicle_width / 2.0; + const double x = t * ego_vel; + const double y = feasible_lat_offset; + + const auto feasible_left_bound_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; + ego_lat_feasible_paths.left_path.push_back(feasible_left_bound_point); + + const auto feasible_right_bound_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; + ego_lat_feasible_paths.right_path.push_back(feasible_right_bound_point); + } + + tier4_autoware_utils::appendMarkerArray( + marker_utils::createPointsMarkerArray( + ego_lat_feasible_paths.left_path, "ego_lat_feasible_left_path", 0, 0.6, 0.9, 0.9), + &debug_marker_); + tier4_autoware_utils::appendMarkerArray( + marker_utils::createPointsMarkerArray( + ego_lat_feasible_paths.right_path, "ego_lat_feasible_right_path", 0, 0.6, 0.9, 0.9), + &debug_marker_); + + return ego_lat_feasible_paths; +} + void DynamicAvoidanceModule::updateRefPathBeforeLaneChange( const std::vector & ego_ref_path_points) { @@ -1251,32 +1326,75 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( ? lon_bound_end_idx_opt.value() : static_cast(ref_path_points_for_obj_poly.size() - 1); - // create inner/outer bound points - std::vector obj_inner_bound_points; - std::vector obj_outer_bound_points; + // create inner bound points + std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - ref_path_points_for_obj_poly.at(i).point.pose, 0.0, - object.lat_offset_to_avoid->min_value, 0.0) - .position); - obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - ref_path_points_for_obj_poly.at(i).point.pose, 0.0, - object.lat_offset_to_avoid->max_value, 0.0) - .position); + // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. + obj_inner_bound_poses.push_back(tier4_autoware_utils::calcOffsetPose( + ref_path_points_for_obj_poly.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, + 0.0)); } - // create obj_polygon from inner/outer bound points - tier4_autoware_utils::Polygon2d obj_poly; - for (const auto & bound_point : obj_inner_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); - obj_poly.outer().push_back(obj_poly_point); + // calculate start index laterally feasible for ego to shift considering maximum lateral jerk and + // acceleration + const auto obj_inner_bound_start_idx = [&]() -> std::optional { + const auto & ego_lat_feasible_path = object.is_collision_left + ? object.ego_lat_feasible_paths.left_path + : object.ego_lat_feasible_paths.right_path; + const auto intersect_result = intersectLines(obj_inner_bound_poses, ego_lat_feasible_path); + + // Check if the object polygon intersects with the ego_lat_feasible_path. + if (intersect_result) { + const auto & [bound_seg_idx, intersect_point] = *intersect_result; + const double lon_offset = tier4_autoware_utils::calcDistance2d( + obj_inner_bound_poses.at(bound_seg_idx), intersect_point); + + const auto obj_inner_bound_start_idx_opt = + motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); + if (obj_inner_bound_start_idx_opt) { + return *obj_inner_bound_start_idx_opt; + } + } + + // Check if the object polygon is fully outside the ego_lat_feasible_path. + const double obj_poly_lat_offset = motion_utils::calcLateralOffset( + ego_lat_feasible_path, obj_inner_bound_poses.front().position); + if ( + (!object.is_collision_left && 0 < obj_poly_lat_offset) || + (object.is_collision_left && obj_poly_lat_offset < 0)) { + return std::nullopt; + } + + return 0; + }(); + if (!obj_inner_bound_start_idx) { + return std::nullopt; } - std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end()); - for (const auto & bound_point : obj_outer_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); - obj_poly.outer().push_back(obj_poly_point); + + // calculate feasible inner/outer bound points + const auto feasible_obj_inner_bound_poses = std::vector( + obj_inner_bound_poses.begin() + *obj_inner_bound_start_idx, obj_inner_bound_poses.end()); + const auto feasible_obj_inner_bound_points = convertToPoints(feasible_obj_inner_bound_poses); + std::vector feasible_obj_outer_bound_points; + for (const auto & feasible_obj_inner_bound_pose : feasible_obj_inner_bound_poses) { + feasible_obj_outer_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + feasible_obj_inner_bound_pose, 0.0, + object.lat_offset_to_avoid->max_value - object.lat_offset_to_avoid->min_value, 0.0) + .position); } + // create obj_polygon from inner/outer bound points + tier4_autoware_utils::Polygon2d obj_poly; + const auto add_points_to_obj_poly = [&](const auto & bound_points) { + for (const auto & bound_point : bound_points) { + obj_poly.outer().push_back(tier4_autoware_utils::Point2d(bound_point.x, bound_point.y)); + } + }; + add_points_to_obj_poly(feasible_obj_inner_bound_points); + std::reverse(feasible_obj_outer_bound_points.begin(), feasible_obj_outer_bound_points.end()); + add_points_to_obj_poly(feasible_obj_outer_bound_points); + boost::geometry::correct(obj_poly); return obj_poly; } diff --git a/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..16b4ffbc688ae --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,123 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/behavior_path_planner_node.hpp" + +#include +#include +#include + +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_dynamic_avoidance_module") + + "/config/dynamic_avoidance.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f3727f35a492f..54b4bf122da00 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,13 +7,9 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) - ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp - src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp - src/scene_module/dynamic_avoidance/manager.cpp ) target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 6f462e17248f0..03f8c8d2a5696 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -28,7 +28,7 @@ Behavior Path Planner has following scene modules | :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------------------- | | Lane Following | this module generates reference path from lanelet centerline. | LINK | | Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_module/README.md) | -| Dynamic Avoidance | WIP | LINK | +| Dynamic Avoidance | WIP | [LINK](../behavior_path_dynamic_avoidance_module/README.md) | | Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | | Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | | External Lane Change | WIP | LINK | diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 28ee6d31e80dc..7ba934e873a8d 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -50,21 +50,18 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml"}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index f34c0266ac081..6426f16a44259 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp @@ -66,6 +66,10 @@ MarkerArray createFootprintMarkerArray( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b); + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index bf48252a01dcd..6380830806c33 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -37,43 +37,6 @@ using autoware_perception_msgs::msg::TrafficSignal; using autoware_perception_msgs::msg::TrafficSignalElement; using geometry_msgs::msg::Pose; -/** - * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. - * - * Iterates through the traffic light elements to find a circle-shaped light that matches the given - * color. - * - * @param tl_state The traffic light state to check. - * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. - * @return True if a circle-shaped light with the specified color is found, false otherwise. - */ -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color); - -/** - * @brief Checks if a traffic light state includes a light with the specified shape. - * - * Searches through the traffic light elements to find a light that matches the given shape. - * - * @param tl_state The traffic light state to check. - * @param shape The shape to look for in the traffic light's lights. - * @return True if a light with the specified shape is found, false otherwise. - */ -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape); - -/** - * @brief Determines if a traffic signal indicates a stop for the given lanelet. - * - * Evaluates the current state of the traffic light, considering if it's green or unknown, - * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet - * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can - * proceed based on allowed turn directions. - * - * @param lanelet The lanelet to check for a stop signal at its traffic light. - * @param tl_state The current state of the traffic light associated with the lanelet. - * @return True if the traffic signal indicates a stop is required, false otherwise. - */ -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state); - /** * @brief Computes the distance from the current position to the next traffic light along a set of * lanelets. diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index 83e9c39e24dd6..db5dc51150e55 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -60,6 +60,7 @@ tf2 tier4_autoware_utils tier4_planning_msgs + traffic_light_utils vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp index 7933c21305432..d97ea2e09a89d 100644 --- a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp @@ -74,7 +74,23 @@ MarkerArray createFootprintMarkerArray( MarkerArray marker_array; addFootprintMarker(marker, base_link_pose, vehicle_info); + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & point : points) { + marker.points.push_back(point); + } + MarkerArray msg; msg.markers.push_back(marker); return msg; } diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index ec147a7bb2771..1aa55e5f350b3 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -31,6 +31,42 @@ namespace { +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Point & target_point) +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + + std::optional closest_idx{std::nullopt}; + double min_lateral_dist = std::numeric_limits::max(); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const double lon_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double lat_dist = [&]() { + if (lon_dist < 0.0) { + return calcDistance2d(points.at(seg_idx), target_point); + } + if (segment_length < lon_dist) { + return calcDistance2d(points.at(seg_idx + 1), target_point); + } + return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + }(); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + min_lateral_dist = lat_dist; + } + } + + if (closest_idx) { + return *closest_idx; + } + + return motion_utils::findNearestSegmentIndex(points, target_point); +} + template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Pose & target_point, @@ -813,9 +849,9 @@ void generateDrivableArea( const auto right_start_point = calcLongitudinalOffsetStartPoint( right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - motion_utils::findNearestSegmentIndex(left_bound, left_start_point); + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); const size_t right_start_idx = - motion_utils::findNearestSegmentIndex(right_bound, right_start_point); + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); // Insert a start point path.left_bound.push_back(left_start_point); @@ -835,9 +871,10 @@ void generateDrivableArea( const auto right_goal_point = calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = std::max( - goal_left_start_idx, motion_utils::findNearestSegmentIndex(left_bound, left_goal_point)); + goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); const size_t right_goal_idx = std::max( - goal_right_start_idx, motion_utils::findNearestSegmentIndex(right_bound, right_goal_point)); + goal_right_start_idx, + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 850919f539e59..f77fafb42d829 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -14,63 +14,12 @@ #include #include +#include namespace behavior_path_planner::utils::traffic_light { using motion_utils::calcSignedArcLength; -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) -{ - const auto it_lamp = - std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; - }); - - return it_lamp != tl_state.elements.end(); -} - -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) -{ - const auto it_lamp = std::find_if( - tl_state.elements.begin(), tl_state.elements.end(), - [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - - return it_lamp != tl_state.elements.end(); -} - -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) -{ - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { - return false; - } - - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { - return false; - } - - const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); - - if (turn_direction == "else") { - return true; - } - if ( - turn_direction == "right" && - hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { - return false; - } - if ( - turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { - return false; - } - if ( - turn_direction == "straight" && - hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { - return false; - } - - return true; -} - double getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { @@ -138,7 +87,8 @@ std::optional calcDistanceToRedTrafficLight( continue; } - if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + if (!traffic_light_utils::isTrafficSignalStop( + lanelet, traffic_signal_stamped.value().signal)) { continue; } diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 71b56bf7288c0..63493f45a7407 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -244,6 +244,52 @@ If a safe path cannot be generated from the current position, search backwards f [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) +### **search priority** + +If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation. + +During this backward search, different policies can be applied based on `search_priority` parameters: + +Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move. +Opting for `short_back_distance` aims to find a location with the least possible backward movement. + +![priority_order](./images/priority_order.drawio.svg) + +`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation. + +#### `efficient_path` + +When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 1 | shift_pull_out | +| ... | ... | +| N | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | geometric_pull_out | + +This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate. + +#### `short_back_distance` + +For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | shift_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | shift_pull_out | +| N | geometric_pull_out | + +This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position. + ### **parameters for backward pull out start point search** | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg new file mode 100644 index 0000000000000..ab48c30b6fdae --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg @@ -0,0 +1,319 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + (0, shift_pull_out) +
+ + (1, shift_pull_out) +
+ (2, shift_pull_out) +
+ (3, shift_pull_out) +
+ (4, shift_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (0, + geometric_pull_out + ) +
+ (1, + geometric_pull_out + ) +
+ (2, + geometric_pull_out + ) +
+ (3, + geometric_pull_out + ) +
+ (4, + geometric_pull_out + ) +
+ ︙ +
+ (N, + geometric_pull_out + ) + +
+ +
+
+ + +
+
backward_search_resolution
+
+ + +
+
geometric pull out path
+
+ + +
+
start pose candidate
+
+ + +
+
idx: 0
+
+ + +
+
idx: 1
+
+ + +
+
idx: 2
+
+ + +
+
idx: 3
+
+ + +
+
idx: 4
+
+ + +
+
shift pull out path
+
+ + +
+
+ (idx, planner_type):= +
+ (3, geometric_pull_out) +
+
+ + +
+
(3, shift_pull_out)
+
+ + +
+
idx: N
+
+ + +
+
+ + efficient_path +
+ +
+
+ + +
+
+ (0, shift_pull_out) +
+ (0, geometric_pull_out) +
+ (1, shift_pull_out) +
+ (1, geometric_pull_out) +
+ (2, shift_pull_out) +
+ (2, geometric_pull_out) +
+ (3, shift_pull_out) +
+ (3, geometric_pull_out) +
+ (4, shift_pull_out) +
+ (4, geometric_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (N, geometric_pull_out) + +
+ +
+
+ + +
+
+ + short_back_distance +
+ +
+
+ + +
+
high priority
+
+ + +
+
low priority
+
+ + diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index d10fc35a5a6f4..0dc7b78b6fe46 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -393,7 +393,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return {}; } - // Check if the ego should stop beyond the stop line. + // Check if the ego should stop at the stop line or the other points. const bool stop_at_stop_line = dist_ego_to_stop < nearest_stop_info->second && nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; @@ -404,9 +404,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return createStopFactor(*default_stop_pose, stop_factor_points); } } else { - // Stop beyond the stop line const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, planner_param_.stop_distance_from_object); + ego_path.points, nearest_stop_info->first, + -base_link2front - planner_param_.stop_distance_from_object); if (stop_pose) { return createStopFactor(*stop_pose, stop_factor_points); } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 631c03d923da0..292a6e6a8843d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,7 +17,11 @@ #include "util.hpp" #include +#include #include +#include +#include +#include #include #include #include @@ -37,10 +41,27 @@ #include #include +#include #include #include #include #include + +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -71,6 +92,25 @@ Polygon2d createOneStepPolygon( } } // namespace +static bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -376,7 +416,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); return; @@ -392,7 +432,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, - [[maybe_unused]] util::DebugData * debug_data) + [[maybe_unused]] DebugData * debug_data) { return; } @@ -403,7 +443,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -451,7 +491,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -483,7 +523,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -526,7 +566,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -577,7 +617,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -634,7 +674,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -681,7 +721,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -734,7 +774,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -776,7 +816,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -818,7 +858,7 @@ void reactRTCApproval( const IntersectionModule::DecisionResult & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { std::visit( VisitorSwitch{[&](const auto & decision) { @@ -868,7 +908,7 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - debug_data_ = util::DebugData(); + debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); // set default RTC @@ -938,6 +978,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const auto & current_pose = planner_data_->current_odometry->pose; // spline interpolation const auto interpolated_path_info_opt = util::generateInterpolatedPath( @@ -952,25 +994,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } // cache intersection lane information because it is invariant - const auto & current_pose = planner_data_->current_odometry->pose; const auto lanelets_on_path = planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); if (!intersection_lanelets_) { - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.common.attention_area_length, - planner_param_.occlusion.occlusion_attention_area_length, - planner_param_.collision_detection.consider_wrong_direction_vehicle); + intersection_lanelets_ = + getObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path); } auto & intersection_lanelets = intersection_lanelets_.value(); - // at the very first time of registration of this module, the path may not be conflicting with the + // at the very first time of regisTration of this module, the path may not be conflicting with the // attention area, so update() is called to update the internal data as well as traffic light info const auto traffic_prioritized_level = - util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); // this is abnormal @@ -985,21 +1022,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // generate all stop line candidates // see the doc for struct IntersectionStopLines - const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); /// even if the attention area is null, stuck vehicle stop line needs to be generated from /// conflicting lanes - const auto & dummy_first_attention_area = - first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; - const auto & dummy_first_attention_lane_centerline = - intersection_lanelets.first_attention_lane() - ? intersection_lanelets.first_attention_lane().value().centerline2d() - : first_conflicting_lane.centerline2d(); - const auto intersection_stoplines_opt = util::generateIntersectionStopLines( - first_conflicting_area, dummy_first_attention_area, dummy_first_attention_lane_centerline, - planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.default_stopline_margin, planner_param_.common.max_accel, - planner_param_.common.max_jerk, planner_param_.common.delay_response_time, - planner_param_.occlusion.peeking_offset, path); + const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() + ? intersection_lanelets.first_attention_lane().value() + : first_conflicting_lane; + + const auto intersection_stoplines_opt = generateIntersectionStopLines( + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, interpolated_path_info, + path); if (!intersection_stoplines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; } @@ -1010,15 +1041,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; // see the doc for struct PathLanelets + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); const auto & conflicting_area = intersection_lanelets.conflicting_area(); - const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, - conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); + const auto path_lanelets_opt = generatePathLanelets( + lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, + first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; } - const auto path_lanelets = path_lanelets_opt.value(); + const auto & path_lanelets = path_lanelets_opt.value(); // utility functions auto fromEgoDist = [&](const size_t index) { @@ -1054,7 +1085,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation - const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets, &debug_data_); const bool is_first_conflicting_lane_private = (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); if (stuck_detected) { @@ -1119,11 +1150,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // check occlusion on detection lane if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( + occlusion_attention_divisions_ = generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution, - planner_param_.occlusion.attention_lane_crop_curvature_threshold, - planner_param_.occlusion.attention_lane_curvature_calculation_ds); + planner_data_->occupancy_grid->info.resolution); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); @@ -1132,8 +1161,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const bool yield_stuck_detected = checkYieldStuckVehicle( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_); + const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding(), + &debug_data_); if (yield_stuck_detected) { std::optional stopline_idx = std::nullopt; const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; @@ -1159,15 +1189,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const double is_amber_or_red = - (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); + (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || + (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); auto occlusion_status = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) ? getOcclusionStatus( - *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area, interpolated_path_info, occlusion_attention_divisions, - target_objects, current_pose, - planner_param_.occlusion.occlusion_required_clearance_distance) + occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, + occlusion_attention_divisions, target_objects) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -1385,9 +1413,672 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } -bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) +TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( + lanelet::ConstLanelet lane, const std::map & tl_infos) +{ + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return TrafficPrioritizedLevel::NOT_PRIORITIZED; + } + const auto tl_info_it = tl_infos.find(tl_id.value()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return TrafficPrioritizedLevel::NOT_PRIORITIZED; + } + const auto & tl_info = tl_info_it->second; + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; + } + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; + } + } + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + } + return TrafficPrioritizedLevel::NOT_PRIORITIZED; +} + +static std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) +{ + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); + } + return polys; +} + +IntersectionLanelets IntersectionModule::getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path) +{ + const double detection_area_length = planner_param_.common.attention_area_length; + const double occlusion_detection_area_length = + planner_param_.occlusion.occlusion_attention_area_length; + const bool consider_wrong_direction_vehicle = + planner_param_.collision_detection.consider_wrong_direction_vehicle; + + // retrieve a stopline associated with a traffic light + bool has_traffic_light = false; + if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + + // for low priority lane + // If ego_lane has right of way (i.e. is high priority), + // ignore yieldLanelets (i.e. low priority lanes) + lanelet::ConstLanelets yield_lanelets{}; + const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); + for (const auto & right_of_way : right_of_ways) { + if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { + for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { + yield_lanelets.push_back(yield_lanelet); + for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { + yield_lanelets.push_back(previous_lanelet); + } + } + } + } + + // get all following lanes of previous lane + lanelet::ConstLanelets ego_lanelets = lanelets_on_path; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + ego_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { + continue; + } + ego_lanelets.push_back(following_lanelet); + } + } + + // get conflicting lanes on assigned lanelet + const auto & conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + std::vector adjacent_followings; + + for (const auto & conflicting_lanelet : conflicting_lanelets) { + for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + } + + // final objective lanelets + lanelet::ConstLanelets detection_lanelets; + lanelet::ConstLanelets conflicting_ex_ego_lanelets; + // conflicting lanes is necessary to get stopline for stuck vehicle + for (auto && conflicting_lanelet : conflicting_lanelets) { + if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) + conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); + } + + // exclude yield lanelets and ego lanelets from detection_lanelets + if (turn_direction_ == std::string("straight") && has_traffic_light) { + // if assigned lanelet is "straight" with traffic light, detection area is not necessary + } else { + if (consider_wrong_direction_vehicle) { + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + for (const auto & adjacent_following : adjacent_followings) { + detection_lanelets.push_back(adjacent_following); + } + } else { + // otherwise we need to know the priority from RightOfWay + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if ( + lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || + lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + } + } + + // get possible lanelet path that reaches conflicting_lane longer than given length + lanelet::ConstLanelets detection_and_preceding_lanelets; + { + const double length = detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(l); + } + } + } + } + + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); + + IntersectionLanelets result; + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stopline{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + break; + } + if (stopline) break; + } + result.attention_stoplines_.push_back(stopline); + } + result.attention_non_preceding_ = std::move(detection_lanelets); + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + std::optional stopline = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + } + result.attention_non_preceding_stoplines_.push_back(stopline); + } + result.conflicting_ = std::move(conflicting_ex_ego_lanelets); + result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched + result.attention_area_ = getPolygon3dFromLanelets(result.attention_); + result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); + result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); + result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); + result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); + return result; +} + +std::optional IntersectionModule::generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const util::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) +{ + const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; + const double stopline_margin = planner_param_.common.default_stopline_margin; + const double max_accel = planner_param_.common.max_accel; + const double max_jerk = planner_param_.common.max_jerk; + const double delay_response_time = planner_param_.common.delay_response_time; + const double peeking_offset = planner_param_.occlusion.peeking_offset; + + const auto first_attention_area = first_attention_lane.polygon3d(); + const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); + const auto & path_ip = interpolated_path_info.path; + const double ds = interpolated_path_info.ds; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); + const int base2front_idx_dist = + std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); + + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); + + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // NOTE: maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + + // (1) default stop line position on interpolated path + bool default_stopline_valid = true; + int stop_idx_ip_int = -1; + if (const auto map_stop_idx_ip = + getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); + map_stop_idx_ip) { + stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; + } + if (stop_idx_ip_int < 0) { + stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; + } + if (stop_idx_ip_int < 0) { + default_stopline_valid = false; + } + const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; + + // (2) ego front stop line position on interpolated path + const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + + // (3) occlusion peeking stop line position on interpolated path + int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); + bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the detection area, invalid + { + const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + occlusion_peeking_line_ip_int = + first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); + } + + const auto occlusion_peeking_line_ip = static_cast( + std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; + const bool first_attention_stopline_valid = true; + + // (4) pass judge line position on interpolated path + const double velocity = planner_data_->current_velocity->twist.linear.x; + const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; + const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); + const auto pass_judge_line_ip = static_cast( + std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + // TODO(Mamoru Sobue): maybe braking dist should be considered + const auto occlusion_wo_tl_pass_judge_line_ip = + static_cast(first_footprint_attention_centerline_ip); + + // (5) stuck vehicle stop line + int stuck_stopline_ip_int = 0; + bool stuck_stopline_valid = true; + if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching detection area and already passed + // first_conflicting_area, this could be null. + const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); + if (!stuck_stopline_idx_ip_opt) { + stuck_stopline_valid = false; + stuck_stopline_ip_int = 0; + } else { + stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; + } + } else { + stuck_stopline_ip_int = + std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); + } + if (stuck_stopline_ip_int < 0) { + stuck_stopline_valid = false; + } + const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stopline{0}; + size_t default_stopline{0}; + size_t first_attention_stopline{0}; + size_t occlusion_peeking_stopline{0}; + size_t pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stoplines_temp; + std::list> stoplines = { + {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, + {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, + {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, + {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, + {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; + stoplines.sort( + [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); + for (const auto & [stop_idx_ip, stop_idx] : stoplines) { + const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; + const auto insert_idx = util::insertPointIndex( + insert_point, original_path, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + if (!insert_idx) { + return std::nullopt; + } + *stop_idx = insert_idx.value(); + } + if ( + intersection_stoplines_temp.occlusion_peeking_stopline < + intersection_stoplines_temp.default_stopline) { + intersection_stoplines_temp.occlusion_peeking_stopline = + intersection_stoplines_temp.default_stopline; + } + + IntersectionStopLines intersection_stoplines; + intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; + if (stuck_stopline_valid) { + intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; + } + if (default_stopline_valid) { + intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; + } + if (first_attention_stopline_valid) { + intersection_stoplines.first_attention_stopline = + intersection_stoplines_temp.first_attention_stopline; + } + if (occlusion_peeking_line_valid) { + intersection_stoplines.occlusion_peeking_stopline = + intersection_stoplines_temp.occlusion_peeking_stopline; + } + intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; + intersection_stoplines.occlusion_wo_tl_pass_judge_line = + intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; + return intersection_stoplines; +} + +/** + * @brief Get stop point from map if exists + * @param stop_pose stop point defined on map + * @return true when the stop point is defined on map. + */ +std::optional IntersectionModule::getStopLineIndexFromMap( + const util::InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) +{ + const auto & path = interpolated_path_info.path; + const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); + + const auto road_markings = + assigned_lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stopline; + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stopline.push_back(road_marking->roadMarking()); + break; // only one stopline exists. + } + } + if (stopline.empty()) { + return std::nullopt; + } + + const auto p_start = stopline.front().front(); + const auto p_end = stopline.front().back(); + const LineString2d extended_stopline = + planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); + + for (size_t i = lane_interval.first; i < lane_interval.second; i++) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + std::vector collision_points; + bg::intersection(extended_stopline, path_segment, collision_points); + + if (collision_points.empty()) { + continue; + } + + return i; + } + + geometry_msgs::msg::Pose stop_point_from_map; + stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); + stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); + stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); + + return motion_utils::findFirstNearestIndexWithSoftConstraints( + path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); +} + +static lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets previous_lanelets; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return previous_lanelets; + } + previous_lanelets.push_back(ll); + } + return previous_lanelets; +} + +// end inclusive +static lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, + const double interval) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + size_t prev_idx = start_idx; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + const auto & p_prev = path.points.at(prev_idx).point.pose; + if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + prev_idx = i; + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + // NOTE: maybe this is opposite + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + +static std::optional> +getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, + const std::vector & polygons, const bool search_forward = true) +{ + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + if (i == 0) { + break; + } + } + } + return std::nullopt; +} + +std::optional IntersectionModule::generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx) { + const double width = planner_data_->vehicle_info_.vehicle_width_m; + static constexpr double path_lanelet_interval = 1.5; + + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; + + PathLanelets path_lanelets; + // prev + path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids_); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } + return path_lanelets; +} + +bool IntersectionModule::checkStuckVehicleInIntersection( + const PathLanelets & path_lanelets, DebugData * debug_data) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getLaneletLength3d; + using lanelet::utils::getPolygonFromArcLength; + using lanelet::utils::to2D; + const bool stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || @@ -1397,22 +2088,146 @@ bool IntersectionModule::checkStuckVehicle( return false; } - const auto & objects_ptr = planner_data->predicted_objects; + const auto & objects_ptr = planner_data_->predicted_objects; // considering lane change in the intersection, these lanelets are generated from the path - const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); + const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; + Polygon2d stuck_vehicle_detect_area{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return false; + } + + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } + const auto target_polygon = + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); + + if (target_polygon.empty()) { + return false; + } + + for (const auto & p : target_polygon) { + stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); + } + + stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); + bg::correct(stuck_vehicle_detect_area); + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - return util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, &debug_data_); + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { + continue; // not stop vehicle + } + + // check if the footprint is in the stuck detect area + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + if (is_in_stuck_area && debug_data) { + debug_data->stuck_targets.objects.push_back(object); + return true; + } + } + return false; } -bool IntersectionModule::checkYieldStuckVehicle( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets) +static lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +{ + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; + } + + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} + +static lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +bool IntersectionModule::checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) { const bool yield_stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || @@ -1423,20 +2238,74 @@ bool IntersectionModule::checkYieldStuckVehicle( return false; } - return util::checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, attention_lanelets, turn_direction_, - planner_data_->vehicle_info_.vehicle_width_m, - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, - planner_param_.yield_stuck.distance_threshold, &debug_data_); + const double width = planner_data_->vehicle_info_.vehicle_width_m; + const double stuck_vehicle_vel_thr = + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; + const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; + + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction_ == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction_ == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { + continue; + } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); + } + debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); + for (const auto & object : target_objects.all_attention_objects) { + const auto obj_v_norm = std::hypot( + object.object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; + } + for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + if (is_in_lanelet) { + debug_data->yield_stuck_targets.objects.push_back(object.object); + return true; + } + } + } + return false; } -util::TargetObjects IntersectionModule::generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, +TargetObjects IntersectionModule::generateTargetObjects( + const IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - util::TargetObjects target_objects; + TargetObjects target_objects; target_objects.header = objects_ptr->header; const auto & attention_lanelets = intersection_lanelets.attention(); const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); @@ -1449,10 +2318,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects( // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, false); + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); if (belong_adjacent_lanelet_id) { continue; } @@ -1466,33 +2333,28 @@ util::TargetObjects IntersectionModule::generateTargetObjects( const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); if (belong_attention_lanelet_id) { const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); container.push_back(target_object); } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = std::nullopt; target_object.stopline = std::nullopt; target_objects.intersection_area_objects.push_back(target_object); } - } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, - planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( + object_direction, attention_lanelets, is_parked_vehicle); belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); @@ -1512,10 +2374,10 @@ util::TargetObjects IntersectionModule::generateTargetObjects( } bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1523,14 +2385,8 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, lane_id_, associative_ids_, closest_idx, - last_intersection_stopline_candidate_idx, time_delay, - planner_param_.collision_detection.velocity_profile.default_velocity, - planner_param_.collision_detection.velocity_profile.minimum_default_velocity, - planner_param_.collision_detection.velocity_profile.use_upstream, - planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity, - &ego_ttc_time_array); + const auto time_distance_array = calcIntersectionPassingTime( + path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); if ( std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != @@ -1540,7 +2396,7 @@ bool IntersectionModule::checkCollision( } const double passing_time = time_distance_array.back().first; - util::cutPredictPathWithDuration(target_objects, clock_, passing_time); + cutPredictPathWithDuration(target_objects, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( @@ -1551,12 +2407,12 @@ bool IntersectionModule::checkCollision( // change TTC margin based on ego traffic light color const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { - if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { return std::make_pair( planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); } - if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { return std::make_pair( planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); @@ -1565,7 +2421,7 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.not_prioritized.collision_start_margin_time, planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); - const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { + const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { if (!target_object.dist_to_stopline) { return false; } @@ -1580,7 +2436,7 @@ bool IntersectionModule::checkCollision( .object_expected_deceleration)); return dist_to_stopline > braking_distance; }; - const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + const auto isTolerableOvershoot = [&](const TargetObject & target_object) { if ( !target_object.attention_lanelet || !target_object.dist_to_stopline || !target_object.stopline) { @@ -1639,14 +2495,14 @@ bool IntersectionModule::checkCollision( // If the vehicle is expected to stop before their stopline, ignore const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); if ( - traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && expected_to_stop_before_stopline) { debug_data_.amber_ignore_targets.objects.push_back(object); continue; } const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); if ( - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && !expected_to_stop_before_stopline && is_tolerable_overshoot) { debug_data_.red_overshoot_ignore_targets.objects.push_back(object); continue; @@ -1766,16 +2622,288 @@ bool IntersectionModule::checkCollision( return collision_detected; } +std::optional IntersectionModule::checkAngleForTargetLanelets( + + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const +{ + const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; + const bool consider_wrong_direction_vehicle = + planner_param_.common.attention_area_angle_threshold; + const double dist_margin = planner_param_.common.attention_area_margin; + + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { + continue; + } + const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); + const double pose_angle = tf2::getYaw(pose.orientation); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + if (consider_wrong_direction_vehicle) { + if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + } else { + if (std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); + } + } + } + return std::nullopt; +} + +void IntersectionModule::cutPredictPathWithDuration( + TargetObjects * target_objects, const double time_thr) +{ + const rclcpp::Time current_time = clock_->now(); + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths + const auto origin_path = predicted_path; + predicted_path.path.clear(); + + for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points + const auto & predicted_pose = origin_path.path.at(k); + const auto predicted_time = + rclcpp::Time(target_objects->header.stamp) + + rclcpp::Duration(origin_path.time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + predicted_path.path.push_back(predicted_pose); + } + } + } + } +} + +TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) +{ + const double intersection_velocity = + planner_param_.collision_detection.velocity_profile.default_velocity; + const double minimum_ego_velocity = + planner_param_.collision_detection.velocity_profile.minimum_default_velocity; + const bool use_upstream_velocity = + planner_param_.collision_detection.velocity_profile.use_upstream; + const double minimum_upstream_velocity = + planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; + const double current_velocity = planner_data_->current_velocity->twist.linear.x; + + int assigned_lane_found = false; + + // crop intersection part of the path, and set the reference velocity to intersection_velocity + // for ego's ttc + PathWithLaneId reference_path; + std::optional upstream_stopline{std::nullopt}; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + auto reference_point = path.points.at(i); + // assume backward velocity is current ego velocity + if (i < closest_idx) { + reference_point.point.longitudinal_velocity_mps = current_velocity; + } + if ( + i > last_intersection_stopline_candidate_idx && + std::fabs(reference_point.point.longitudinal_velocity_mps) < + std::numeric_limits::epsilon() && + !upstream_stopline) { + upstream_stopline = i; + } + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } + reference_path.points.push_back(reference_point); + bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); + if (assigned_lane_found && !has_objective_lane_id) { + break; + } + assigned_lane_found = has_objective_lane_id; + } + if (!assigned_lane_found) { + return {{0.0, 0.0}}; // has already passed the intersection. + } + + std::vector> original_path_xy; + for (size_t i = 0; i < reference_path.points.size(); ++i) { + const auto & p = reference_path.points.at(i).point.pose.position; + original_path_xy.emplace_back(p.x, p.y); + } + + // apply smoother to reference velocity + PathWithLaneId smoothed_reference_path = reference_path; + if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { + smoothed_reference_path = reference_path; + } + + // calculate when ego is going to reach each (interpolated) points on the path + TimeDistanceArray time_distance_array{}; + double dist_sum = 0.0; + double passing_time = time_delay; + time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stopline_candidate_idx` makes no sense + const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + + const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { + if (upstream_stopline) { + const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, upstream_stopline_point, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + } else { + return std::nullopt; + } + }(); + const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); + const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); + + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + const auto & p1 = smoothed_reference_path.points.at(i); + const auto & p2 = smoothed_reference_path.points.at(i + 1); + + const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + dist_sum += dist; + + // use average velocity between p1 and p2 + const double average_velocity = + (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; + const double passing_velocity = [=]() { + if (use_upstream_velocity) { + if (has_upstream_stopline && i > upstream_stopline_ind) { + return minimum_upstream_velocity; + } + return std::max(average_velocity, minimum_ego_velocity); + } else { + return std::max(average_velocity, minimum_ego_velocity); + } + }(); + passing_time += (dist / passing_velocity); + + time_distance_array.emplace_back(passing_time, dist_sum); + } + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + debug_ttc_array->data.reserve( + time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id_); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.x); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.y); + } + return time_distance_array; +} + +static double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + +std::vector IntersectionModule::generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) +{ + const double curvature_threshold = + planner_param_.occlusion.attention_lane_crop_curvature_threshold; + const double curvature_calculation_ds = + planner_param_.occlusion.attention_lane_curvature_calculation_ds; + + using lanelet::utils::getCenterlineWithOffset; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { + continue; + } + detection_lanelets.push_back(detection_lanelet); + } + + // (1) tsort detection_lanelets + const auto [merged_detection_lanelets, originals] = + util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); + double area = 0; + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); + } + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); + } + + // (3) discretize each merged lanelet + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); + const double width = area / length; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); + } + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); + } + return detection_divisions; +} + IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr) + const TargetObjects & target_objects) { + const auto & occ_grid = *planner_data_->occupancy_grid; + const auto & current_pose = planner_data_->current_odometry->pose; + const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; + const auto & path_ip = interpolated_path_info.path; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); @@ -2094,6 +3222,76 @@ IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( return OcclusionType::STATICALLY_OCCLUDED; } +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + +void IntersectionLanelets::update( + const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + is_prioritized_ = is_prioritized; + // find the first conflicting/detection area polygon intersecting the path + if (!first_conflicting_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); + } + } + if (!first_attention_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); + } + } +} + +void TargetObject::calc_dist_to_stopline() +{ + if (!attention_lanelet || !stopline) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stopline_val = stopline.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; + stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; + stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); +} + /* bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 2f8c296a348b9..6a350e66706e6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -42,6 +43,259 @@ namespace behavior_velocity_planner using TimeDistanceArray = std::vector>; +struct DebugData +{ + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + std::optional occlusion_first_stop_wall_pose{std::nullopt}; + std::optional pass_judge_wall_pose{std::nullopt}; + std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; + std::vector candidate_collision_object_polygons; + autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + std::vector occlusion_polygons; + std::optional> + nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief see the document for more details of IntersectionLanelets + */ +struct IntersectionLanelets +{ +public: + /** + * update conflicting lanelets and traffic priority information + */ + void update( + const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + + const lanelet::ConstLanelets & attention() const + { + return is_prioritized_ ? attention_non_preceding_ : attention_; + } + const std::vector> & attention_stoplines() const + { + return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; + } + const lanelet::ConstLanelets & conflicting() const { return conflicting_; } + const lanelet::ConstLanelets & adjacent() const { return adjacent_; } + const lanelet::ConstLanelets & occlusion_attention() const + { + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; + } + const lanelet::ConstLanelets & attention_non_preceding() const + { + return attention_non_preceding_; + } + const std::vector & attention_area() const + { + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; + } + const std::vector & conflicting_area() const + { + return conflicting_area_; + } + const std::vector & adjacent_area() const { return adjacent_area_; } + const std::vector & occlusion_attention_area() const + { + return occlusion_attention_area_; + } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } + const std::optional & first_conflicting_area() const + { + return first_conflicting_area_; + } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } + const std::optional & first_attention_area() const + { + return first_attention_area_; + } + + /** + * the set of attention lanelets which is topologically merged + */ + lanelet::ConstLanelets attention_; + std::vector attention_area_; + + /** + * the stop lines for each attention_ lanelets + */ + std::vector> attention_stoplines_; + + /** + * the conflicting part of attention lanelets + */ + lanelet::ConstLanelets attention_non_preceding_; + std::vector attention_non_preceding_area_; + + /** + * the stop lines for each attention_non_preceding_ + */ + std::vector> attention_non_preceding_stoplines_; + + /** + * the conflicting lanelets of the objective intersection lanelet + */ + lanelet::ConstLanelets conflicting_; + std::vector conflicting_area_; + + /** + * + */ + lanelet::ConstLanelets adjacent_; + std::vector adjacent_area_; + + /** + * the set of attention lanelets for occlusion detection which is topologically merged + */ + lanelet::ConstLanelets occlusion_attention_; + std::vector occlusion_attention_area_; + + /** + * the vector of sum of each occlusion_attention lanelet + */ + std::vector occlusion_attention_size_; + + /** + * the attention lanelet which ego path points intersect for the first time + */ + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + + /** + * the attention lanelet which ego path points intersect for the first time + */ + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; + + /** + * flag if the intersection is prioritized by the traffic light + */ + bool is_prioritized_{false}; +}; + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct IntersectionStopLines +{ + size_t closest_idx{0}; + + /** + * stuck_stopline is null if ego path does not intersect with first_conflicting_area + */ + std::optional stuck_stopline{std::nullopt}; + + /** + * default_stopline is null if it is calculated negative from first_attention_stopline + */ + std::optional default_stopline{std::nullopt}; + + /** + * first_attention_stopline is null if ego footprint along the path does not intersect with + * attention area. if path[0] satisfies the condition, it is 0 + */ + std::optional first_attention_stopline{std::nullopt}; + + /** + * occlusion_peeking_stopline is null if path[0] is already inside the attention area + */ + std::optional occlusion_peeking_stopline{std::nullopt}; + + /** + * pass_judge_line is before first_attention_stop_line by the braking distance. if its value is + * calculated negative, it is 0 + */ + size_t pass_judge_line{0}; + + /** + * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with + * the centerline of the first_attention_lane + */ + size_t occlusion_wo_tl_pass_judge_line{0}; +}; + +/** + * @struct + * @brief see the document for more details of PathLanelets + */ +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the + // path +}; + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + std::optional dist_to_stopline{std::nullopt}; + void calc_dist_to_stopline(); +}; + +/** + * @struct + * @brief categorize TargetObjects + */ +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy +}; + +/** + * @struct + * @brief categorize traffic light priority + */ +enum class TrafficPrioritizedLevel { + //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + //! The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + //! The target lane's traffic signal is green + NOT_PRIORITIZED +}; + class IntersectionModule : public SceneModuleInterface { public: @@ -159,33 +413,57 @@ class IntersectionModule : public SceneModuleInterface }; enum OcclusionType { + //! occlusion is not detected NOT_OCCLUDED, + //! occlusion is not caused by dynamic objects STATICALLY_OCCLUDED, + //! occlusion is caused by dynamic objects DYNAMICALLY_OCCLUDED, - RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC + //! actual occlusion does not exist, only disapproved by RTC + RTC_OCCLUDED, }; + /** + * @struct + * @brief Internal error or ego already passed pass_judge_line + */ struct Indecisive { std::string error; }; + /** + * @struct + * @brief detected stuck vehicle + */ struct StuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; std::optional occlusion_stopline_idx{std::nullopt}; }; + /** + * @struct + * @brief yielded by vehicle on the attention area + */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; }; + /** + * @struct + * @brief only collision is detected + */ struct NonOccludedCollisionStop { size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; + /** + * @struct + * @brief occlusion is detected so ego needs to stop at the default stop line position + */ struct FirstWaitBeforeOcclusion { bool is_actually_occlusion_cleared{false}; @@ -193,23 +471,29 @@ class IntersectionModule : public SceneModuleInterface size_t first_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; - // A state peeking to occlusion limit line in the presence of traffic light + /** + * @struct + * @brief ego is approaching the boundary of attention area in the presence of traffic light + */ struct PeekingTowardOcclusion { - // NOTE: if intersection_occlusion is disapproved externally through RTC, - // it indicates "is_forcefully_occluded" + //! if intersection_occlusion is disapproved externally through RTC, it indicates + //! "is_forcefully_occluded" bool is_actually_occlusion_cleared{false}; bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t first_attention_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) - // if valid, it contains the remaining time to release the static occlusion stuck and shows up - // intersection_occlusion(x.y) + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck and shows up + //! intersection_occlusion(x.y) std::optional static_occlusion_timeout{std::nullopt}; }; - // A state detecting both collision and occlusion in the presence of traffic light + /** + * @struct + * @brief both collision and occlusion are detected in the presence of traffic light + */ struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; @@ -218,10 +502,14 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stopline_idx{0}; size_t first_attention_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) - // if valid, it contains the remaining time to release the static occlusion stuck + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; }; + /** + * @struct + * @brief at least occlusion is detected in the absence of traffic light + */ struct OccludedAbsenceTrafficLight { bool is_actually_occlusion_cleared{false}; @@ -231,13 +519,20 @@ class IntersectionModule : public SceneModuleInterface size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; }; + /** + * @struct + * @brief both collision and occlusion are not detected + */ struct Safe { - // NOTE: if RTC is disapproved status, default stop lines are still needed. size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; + /** + * @struct + * @brief traffic light is red or arrow signal + */ struct FullyPrioritized { bool collision_detected{false}; @@ -246,16 +541,16 @@ class IntersectionModule : public SceneModuleInterface size_t occlusion_stopline_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line - StuckStop, // detected stuck vehicle - YieldStuckStop, // detected yield stuck vehicle - NonOccludedCollisionStop, // detected collision while FOV is clear - FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion - PeekingTowardOcclusion, // peeking into occlusion while collision is not detected - OccludedCollisionStop, // occlusion and collision are both detected - OccludedAbsenceTrafficLight, // occlusion is detected in the absence of traffic light - Safe, // judge as safe - FullyPrioritized // only detect vehicles violating traffic rules + Indecisive, //! internal process error, or over the pass judge line + StuckStop, //! detected stuck vehicle + YieldStuckStop, //! detected yield stuck vehicle + NonOccludedCollisionStop, //! detected collision while FOV is clear + FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion + PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected + OccludedCollisionStop, //! occlusion and collision are both detected + OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light + Safe, //! judge as safe + FullyPrioritized //! only detect vehicles violating traffic rules >; IntersectionModule( @@ -265,10 +560,6 @@ class IntersectionModule : public SceneModuleInterface const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - /** - * @brief plan go-stop velocity at traffic crossing with collision check between reference path - * and object predicted path - */ bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; @@ -289,28 +580,27 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; + // Parameter + PlannerParam planner_param_; + + std::optional intersection_lanelets_{std::nullopt}; + + // for pass judge decision bool is_go_out_{false}; bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; OcclusionType prev_occlusion_status_; - // Parameter - PlannerParam planner_param_; - - std::optional intersection_lanelets_{std::nullopt}; - // for occlusion detection const bool enable_occlusion_detection_; std::optional> occlusion_attention_divisions_{ - std::nullopt}; - StateMachine collision_state_machine_; //! for stable collision checking - StateMachine before_creep_state_machine_; //! for two phase stop - StateMachine occlusion_stop_state_machine_; + std::nullopt}; //! for caching discretized occlusion detection lanelets + StateMachine collision_state_machine_; //! for stable collision checking + StateMachine before_creep_state_machine_; //! for two phase stop + StateMachine occlusion_stop_state_machine_; //! for stable occlusion detection StateMachine temporal_stop_before_attention_state_machine_; StateMachine static_occlusion_timeout_state_machine_; - // for pseudo-collision detection when ego just entered intersection on green light and upcoming - // vehicles are very slow std::optional initial_green_light_observed_time_{std::nullopt}; // for RTC @@ -318,43 +608,135 @@ class IntersectionModule : public SceneModuleInterface bool occlusion_safety_{true}; double occlusion_stop_distance_{0.0}; bool occlusion_activated_{true}; - // for first stop in two-phase stop bool occlusion_first_stop_required_{false}; + /** + * @fn + * @brief set all RTC variable to safe and -inf + */ void initializeRTCStatus(); + /** + * @fn + * @brief analyze traffic_light/occupancy/objects context and return DecisionResult + */ + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @fn + * @brief set RTC value according to calculated DecisionResult + */ void prepareRTCStatus( const DecisionResult &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @fn + * @brief find TrafficPrioritizedLevel + */ + TrafficPrioritizedLevel getTrafficPrioritizedLevel( + lanelet::ConstLanelet lane, const std::map & tl_infos); + + /** + * @fn + * @brief generate IntersectionLanelets + */ + IntersectionLanelets getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path); - bool checkStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets); + /** + * @fn + * @brief generate IntersectionStopLines + */ + std::optional generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const util::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - bool checkYieldStuckVehicle( - const util::TargetObjects & target_objects, + /** + * @fn + * @brief find the associated stopline road marking of assigned lanelet + */ + std::optional getStopLineIndexFromMap( const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets); + lanelet::ConstLanelet assigned_lanelet); - util::TargetObjects generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, + /** + * @fn + * @brief generate PathLanelets + */ + std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx); + + /** + * @fn + * @brief check stuck + */ + bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets, DebugData * debug_data); + + /** + * @fn + * @brief check yield stuck + */ + bool checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data); + + /** + * @fn + * @brief categorize target objects + */ + TargetObjects generateTargetObjects( + const IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; + /** + * @fn + * @brief check collision + */ bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level); + + std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const; + + void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); + /** + * @fn + * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of + * (time of arrival, traveled distance) from current ego position + */ + TimeDistanceArray calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); + + std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + + /** + * @fn + * @brief check occlusion status + */ OcclusionType getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr); + const TargetObjects & target_objects); /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -364,7 +746,7 @@ class IntersectionModule : public SceneModuleInterface const double assumed_front_car_decel); */ - util::DebugData debug_data_; + DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index b373f2cbc1c8a..b929959e43f12 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,17 +16,16 @@ #include "util.hpp" -#include #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -50,6 +49,32 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( state_machine_.setState(StateMachine::State::STOP); } +static std::optional getFirstConflictingLanelet( + const lanelet::ConstLanelets & conflicting_lanelets, + const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (const auto & conflicting_lanelet : conflicting_lanelets) { + const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); + const bool intersects = bg::intersects(polygon_2d, path_footprint); + if (intersects) { + return std::make_optional(conflicting_lanelet); + } + } + } + return std::nullopt; +} + bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = DebugData(); @@ -83,44 +108,37 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return false; } - /* get detection area */ - if (!intersection_lanelets_) { - const auto & assigned_lanelet = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); - const auto lanelets_on_path = planning_utils::getLaneletsOnPath( - *path, lanelet_map_ptr, planner_data_->current_odometry->pose); - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, - planner_param_.consider_wrong_direction_vehicle); - } - auto & intersection_lanelets = intersection_lanelets_.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update( - false, interpolated_path_info, local_footprint, - planner_data_->vehicle_info_.max_longitudinal_offset_m); - const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); - if (!first_conflicting_area) { + if (!first_conflicting_lanelet_) { + const auto conflicting_lanelets = getAttentionLanelets(); + first_conflicting_lanelet_ = getFirstConflictingLanelet( + conflicting_lanelets, interpolated_path_info, local_footprint, baselink2front); + } + if (!first_conflicting_lanelet_) { return false; } + const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); - /* set stop-line and stop-judgement-line for base_link */ - const auto stopline_idx_opt = util::generateStuckStopLine( - first_conflicting_area.value(), planner_data_, interpolated_path_info, - planner_param_.stopline_margin, false, path); - if (!stopline_idx_opt.has_value()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); + const auto first_conflicting_idx_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, baselink2front); + if (!first_conflicting_idx_opt) { return false; } - - const size_t stopline_idx = stopline_idx_opt.value(); - if (stopline_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); + const auto stopline_idx_ip = static_cast(std::max( + 0, static_cast(first_conflicting_idx_opt.value()) - + static_cast(baselink2front / planner_param_.path_interpolation_ds))); + + const auto stopline_idx_opt = util::insertPointIndex( + interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + if (!stopline_idx_opt) { + RCLCPP_DEBUG(logger_, "failed to insert stopline, ignore planning."); return true; } + const auto stopline_idx = stopline_idx_opt.value(); - debug_data_.virtual_wall_pose = planning_utils::getAheadPose( - stopline_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); debug_data_.stop_point_pose = path->points.at(stopline_idx).point.pose; /* set stop speed */ @@ -154,45 +172,33 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return true; } -autoware_auto_planning_msgs::msg::PathWithLaneId -MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length) +lanelet::ConstLanelets MergeFromPrivateRoadModule::getAttentionLanelets() const { - if (path.points.size() < 2) { - return path; - } - - autoware_auto_planning_msgs::msg::PathWithLaneId private_path = path; - private_path.points.clear(); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - double sum_dist = 0.0; - bool prev_has_target_lane_id = false; - for (int i = static_cast(path.points.size()) - 2; i >= 0; i--) { - bool has_target_lane_id = false; - for (const auto path_id : path.points.at(i).lane_ids) { - if (path_id == lane_id_) { - has_target_lane_id = true; + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + lanelet::ConstLanelets sibling_lanelets; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + sibling_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(sibling_lanelets, following_lanelet)) { + continue; } + sibling_lanelets.push_back(following_lanelet); } - if (has_target_lane_id) { - // add path point with target lane id - // (lanelet with target lane id is exit of private road) - private_path.points.emplace_back(path.points.at(i)); - prev_has_target_lane_id = true; + } + + lanelet::ConstLanelets attention_lanelets; + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(sibling_lanelets, conflicting_lanelet)) { continue; } - if (prev_has_target_lane_id) { - // extend path to the front - private_path.points.emplace_back(path.points.at(i)); - sum_dist += tier4_autoware_utils::calcDistance2d( - path.points.at(i).point.pose, path.points.at(i + 1).point.pose); - if (sum_dist > extend_length) { - break; - } - } + attention_lanelets.push_back(conflicting_lanelet); } - - std::reverse(private_path.points.begin(), private_path.points.end()); - return private_path; + return attention_lanelets; } + } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index fab0303640700..a44b99c97457d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,10 +15,7 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include "scene_intersection.hpp" - #include -#include #include #include @@ -27,9 +24,6 @@ #include #include -#include -#include - #include #include #include @@ -79,17 +73,15 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface motion_utils::VirtualWalls createVirtualWalls() override; const std::set & getAssociativeIds() const { return associative_ids_; } + lanelet::ConstLanelets getAttentionLanelets() const; private: const int64_t lane_id_; const std::set associative_ids_; - autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length); - // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional first_conflicting_lanelet_; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index e491d2ce7c5ce..6afc2d61d77f2 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -18,15 +18,10 @@ #include #include -#include #include -#include -#include -#include #include #include #include -#include #include #include @@ -38,29 +33,12 @@ #include #include #include -#include #include #include #include -#include #include #include -namespace tier4_autoware_utils -{ - -template <> -inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) -{ - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - point.z = p.z(); - return point; -} - -} // namespace tier4_autoware_utils - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -84,7 +62,7 @@ static std::optional getDuplicatedPointIdx( return std::nullopt; } -static std::optional insertPointIndex( +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) @@ -156,66 +134,7 @@ std::optional> findLaneIdsInterval( return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; } -/** - * @brief Get stop point from map if exists - * @param stop_pose stop point defined on map - * @return true when the stop point is defined on map. - */ -static std::optional getStopLineIndexFromMap( - const InterpolatedPathInfo & interpolated_path_info, - const std::shared_ptr & planner_data) -{ - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - - lanelet::ConstLanelet lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get( - interpolated_path_info.lane_id); - const auto road_markings = lanelet.regulatoryElementsAs(); - lanelet::ConstLineStrings3d stopline; - for (const auto & road_marking : road_markings) { - const std::string type = - road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); - if (type == lanelet::AttributeValueString::StopLine) { - stopline.push_back(road_marking->roadMarking()); - break; // only one stopline exists. - } - } - if (stopline.empty()) { - return std::nullopt; - } - - const auto p_start = stopline.front().front(); - const auto p_end = stopline.front().back(); - const LineString2d extended_stopline = - planning_utils::extendLine(p_start, p_end, planner_data->stop_line_extend_length); - - for (size_t i = lane_interval.first; i < lane_interval.second; i++) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - std::vector collision_points; - bg::intersection(extended_stopline, path_segment, collision_points); - - if (collision_points.empty()) { - continue; - } - - return i; - } - - geometry_msgs::msg::Pose stop_point_from_map; - stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); - stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); - stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - - return motion_utils::findFirstNearestIndexWithSoftConstraints( - path.points, stop_point_from_map, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::optional getFirstPointInsidePolygonByFootprint( +std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { @@ -236,222 +155,6 @@ static std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } -static std::optional> -getFirstPointInsidePolygonsByFootprint( - const std::vector & polygons, - const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - - for (size_t i = start; i <= lane_end; ++i) { - const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (size_t j = 0; j < polygons.size(); ++j) { - const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); - const bool is_in_polygon = bg::intersects(area_2d, path_footprint); - if (is_in_polygon) { - return std::make_optional>(i, j); - } - } - } - return std::nullopt; -} - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const double baselink2front = planner_data->vehicle_info_.max_longitudinal_offset_m; - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - - // find the index of the first point whose vehicle footprint on it intersects with detection_area - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - std::optional first_footprint_inside_detection_ip_opt = - getFirstPointInsidePolygonByFootprint( - first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_detection_ip_opt) { - return std::nullopt; - } - const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); - - std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; - for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // NOTE: maybe consideration of braking dist is necessary - first_footprint_attention_centerline_ip_opt = i; - break; - } - } - if (!first_footprint_attention_centerline_ip_opt) { - return std::nullopt; - } - const size_t first_footprint_attention_centerline_ip = - first_footprint_attention_centerline_ip_opt.value(); - - // (1) default stop line position on interpolated path - bool default_stopline_valid = true; - int stop_idx_ip_int = -1; - if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data); - map_stop_idx_ip) { - stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } - if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; - } - if (stop_idx_ip_int < 0) { - default_stopline_valid = false; - } - const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; - - // (2) ego front stop line position on interpolated path - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( - path_ip.points, current_pose, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - - // (3) occlusion peeking stop line position on interpolated path - int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); - bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the detection area, invalid - { - const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { - occlusion_peeking_line_valid = false; - } - } - if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = - first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); - } - - const auto occlusion_peeking_line_ip = static_cast( - std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; - const bool first_attention_stopline_valid = true; - - // (4) pass judge line position on interpolated path - const double velocity = planner_data->current_velocity->twist.linear.x; - const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; - const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_accel, max_jerk, delay_response_time); - int pass_judge_ip_int = - static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); - const auto pass_judge_line_ip = static_cast( - std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - // TODO(Mamoru Sobue): maybe braking dist should be considered - const auto occlusion_wo_tl_pass_judge_line_ip = - static_cast(first_footprint_attention_centerline_ip); - - // (5) stuck vehicle stop line - int stuck_stopline_ip_int = 0; - bool stuck_stopline_valid = true; - if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching detection area and already passed - // first_conflicting_area, this could be null. - const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); - if (!stuck_stopline_idx_ip_opt) { - stuck_stopline_valid = false; - stuck_stopline_ip_int = 0; - } else { - stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; - } - } else { - stuck_stopline_ip_int = - std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); - } - if (stuck_stopline_ip_int < 0) { - stuck_stopline_valid = false; - } - const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); - - struct IntersectionStopLinesTemp - { - size_t closest_idx{0}; - size_t stuck_stopline{0}; - size_t default_stopline{0}; - size_t first_attention_stopline{0}; - size_t occlusion_peeking_stopline{0}; - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; - }; - - IntersectionStopLinesTemp intersection_stoplines_temp; - std::list> stoplines = { - {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, - {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, - {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, - {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, - {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, - {&occlusion_wo_tl_pass_judge_line_ip, - &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; - stoplines.sort( - [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); - for (const auto & [stop_idx_ip, stop_idx] : stoplines) { - const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; - const auto insert_idx = insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - if (!insert_idx) { - return std::nullopt; - } - *stop_idx = insert_idx.value(); - } - if ( - intersection_stoplines_temp.occlusion_peeking_stopline < - intersection_stoplines_temp.default_stopline) { - intersection_stoplines_temp.occlusion_peeking_stopline = - intersection_stoplines_temp.default_stopline; - } - - IntersectionStopLines intersection_stoplines; - intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; - if (stuck_stopline_valid) { - intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; - } - if (default_stopline_valid) { - intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; - } - if (first_attention_stopline_valid) { - intersection_stoplines.first_attention_stopline = - intersection_stoplines_temp.first_attention_stopline; - } - if (occlusion_peeking_line_valid) { - intersection_stoplines.occlusion_peeking_stopline = - intersection_stoplines_temp.occlusion_peeking_stopline; - } - intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; - intersection_stoplines.occlusion_wo_tl_pass_judge_line = - intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; - return intersection_stoplines; -} - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, @@ -490,105 +193,7 @@ std::optional getFirstPointInsidePolygon( return std::nullopt; } -static std::optional> -getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, - const std::vector & polygons, const bool search_forward = true) -{ - if (search_forward) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - } - } else { - for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - if (i == 0) { - break; - } - } - } - return std::nullopt; -} - -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const auto lane_interval_ip_start = std::get<0>(lane_interval_ip); - size_t stuck_stopline_idx_ip = 0; - if (use_stuck_stopline) { - stuck_stopline_idx_ip = lane_interval_ip_start; - } else { - const auto stuck_stopline_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, conflicting_area); - if (!stuck_stopline_idx_ip_opt) { - return std::nullopt; - } - stuck_stopline_idx_ip = stuck_stopline_idx_ip_opt.value(); - } - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stopline_idx_ip) - 1 - stopline_margin_idx_dist - base2front_idx_dist, - 0)); - const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - return insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::vector getPolygon3dFromLanelets( - const lanelet::ConstLanelets & ll_vec) -{ - std::vector polys; - for (auto && ll : ll_vec) { - polys.push_back(ll.polygon3d()); - } - return polys; -} - -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - -/** - * @param pair lanelets and the vector of original lanelets in topological order (not reversed as - *in generateDetectionLaneDivisions()) - **/ -static std::pair> +std::pair> mergeLaneletsByTopologicalSort( const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr) @@ -683,207 +288,9 @@ mergeLaneletsByTopologicalSort( return {merged, originals}; } -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle) -{ - const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - - // retrieve a stopline associated with a traffic light - bool has_traffic_light = false; - if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); - tl_reg_elems.size() != 0) { - const auto tl_reg_elem = tl_reg_elems.front(); - const auto stopline_opt = tl_reg_elem->stopLine(); - if (!!stopline_opt) has_traffic_light = true; - } - - // for low priority lane - // If ego_lane has right of way (i.e. is high priority), - // ignore yieldLanelets (i.e. low priority lanes) - lanelet::ConstLanelets yield_lanelets{}; - const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); - for (const auto & right_of_way : right_of_ways) { - if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { - for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { - yield_lanelets.push_back(yield_lanelet); - for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { - yield_lanelets.push_back(previous_lanelet); - } - } - } - } - - // get all following lanes of previous lane - lanelet::ConstLanelets ego_lanelets = lanelets_on_path; - for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { - ego_lanelets.push_back(previous_lanelet); - for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { - if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { - continue; - } - ego_lanelets.push_back(following_lanelet); - } - } - - // get conflicting lanes on assigned lanelet - const auto & conflicting_lanelets = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); - std::vector adjacent_followings; - - for (const auto & conflicting_lanelet : conflicting_lanelets) { - for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - } - - // final objective lanelets - lanelet::ConstLanelets detection_lanelets; - lanelet::ConstLanelets conflicting_ex_ego_lanelets; - // conflicting lanes is necessary to get stopline for stuck vehicle - for (auto && conflicting_lanelet : conflicting_lanelets) { - if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) - conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); - } - - // exclude yield lanelets and ego lanelets from detection_lanelets - if (turn_direction == std::string("straight") && has_traffic_light) { - // if assigned lanelet is "straight" with traffic light, detection area is not necessary - } else { - if (consider_wrong_direction_vehicle) { - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - for (const auto & adjacent_following : adjacent_followings) { - detection_lanelets.push_back(adjacent_following); - } - } else { - // otherwise we need to know the priority from RightOfWay - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if ( - lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || - lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - } - } - - // get possible lanelet path that reaches conflicting_lane longer than given length - lanelet::ConstLanelets detection_and_preceding_lanelets; - { - const double length = detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); - } - } - } - } - - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; - { - const double length = occlusion_detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); - } - } - } - } - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; - for (const auto & ll : occlusion_detection_and_preceding_lanelets) { - const auto turn_direction = getTurnDirection(ll); - if (turn_direction == "left" || turn_direction == "right") { - continue; - } - occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); - } - - auto [attention_lanelets, original_attention_lanelet_sequences] = - mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - - IntersectionLanelets result; - result.attention_ = std::move(attention_lanelets); - for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { - // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that - // back() exists. - std::optional stopline{std::nullopt}; - for (auto it = original_attention_lanelet_seq.rbegin(); - it != original_attention_lanelet_seq.rend(); ++it) { - const auto traffic_lights = it->regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - break; - } - if (stopline) break; - } - result.attention_stoplines_.push_back(stopline); - } - result.attention_non_preceding_ = std::move(detection_lanelets); - for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - std::optional stopline = std::nullopt; - const auto & ll = result.attention_non_preceding_.at(i); - const auto traffic_lights = ll.regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - } - result.attention_non_preceding_stoplines_.push_back(stopline); - } - result.conflicting_ = std::move(conflicting_ex_ego_lanelets); - result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - // NOTE: occlusion_attention is not inverted here - // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and - // then trim part of them based on curvature threshold - result.occlusion_attention_ = - std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); - - // NOTE: to properly update(), each element in conflicting_/conflicting_area_, - // attention_non_preceding_/attention_non_preceding_area_ need to be matched - result.attention_area_ = getPolygon3dFromLanelets(result.attention_); - result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); - result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); - result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); - result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); - return result; -} - bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -893,8 +300,8 @@ bool isOverTargetIndex( } bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -903,81 +310,13 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } -/* -static std::vector getAllAdjacentLanelets( - const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) -{ - std::set results; - - results.insert(lane.id()); - - auto it = routing_graph->adjacentRight(lane); - // take all lane on the right side - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentRight(it.get()); - } - // take all lane on the left side - it = routing_graph->adjacentLeft(lane); - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentLeft(it.get()); - - } - return std::vector(results.begin(), results.end()); -} -*/ - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( - lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, - lanelet::ConstLanelet lane) -{ - // some of the intersections are not well-formed, and "adjacent" turning - // lanelets are not sharing the LineStrings - const std::string turn_direction = getTurnDirection(lane); - if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight") - return {}; - - std::set previous_lanelet_ids; - for (auto && previous_lanelet : routing_graph->previous(lane)) { - previous_lanelet_ids.insert(previous_lanelet.id()); - } - - std::set besides_previous_lanelet_ids; - for (auto && previous_lanelet_id : previous_lanelet_ids) { - lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); - for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) { - besides_previous_lanelet_ids.insert(beside_lanelet); - } - } - - std::set following_turning_lanelets; - following_turning_lanelets.insert(lane.id()); - for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { - lanelet::ConstLanelet besides_previous_lanelet = - map->laneletLayer.get(besides_previous_lanelet_id); - for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) { - // if this has {"turn_direction", "${turn_direction}"}, take this - if (getTurnDirection(following_lanelet) == turn_direction) - following_turning_lanelets.insert(following_lanelet.id()); - } - } - lanelet::ConstLanelets ret{}; - for (auto && id : following_turning_lanelets) { - ret.push_back(map->laneletLayer.get(id)); - } - return ret; -} -*/ - -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); if (area_id_str == "else") return std::nullopt; - const int area_id = std::atoi(area_id_str.c_str()); + const lanelet::Id area_id = std::atoi(area_id_str.c_str()); const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); Polygon2d poly{}; for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); @@ -994,111 +333,6 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos) -{ - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; - } - if (!tl_id) { - // this lane has no traffic light - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { - // the info of this traffic light is not available - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_it->second; - bool has_amber_signal{false}; - for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color == TrafficSignalElement::AMBER) { - has_amber_signal = true; - } - if (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; - } - } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; - } - return TrafficPrioritizedLevel::NOT_PRIORITIZED; -} - -double getHighestCurvature(const lanelet::ConstLineString3d & centerline) -{ - std::vector points; - for (auto point = centerline.begin(); point != centerline.end(); point++) { - points.push_back(*point); - } - - SplineInterpolationPoints2d interpolation(points); - const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); - std::vector curvatures_positive; - for (const auto & curvature : curvatures) { - curvatures_positive.push_back(std::fabs(curvature)); - } - return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); -} - -std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds) -{ - using lanelet::utils::getCenterlineWithOffset; - - // (0) remove left/right lanelet - lanelet::ConstLanelets detection_lanelets; - for (const auto & detection_lanelet : detection_lanelets_all) { - // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet - const auto fine_centerline = - lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); - const double highest_curvature = getHighestCurvature(fine_centerline); - if (highest_curvature > curvature_threshold) { - continue; - } - detection_lanelets.push_back(detection_lanelet); - } - - // (1) tsort detection_lanelets - const auto [merged_detection_lanelets, originals] = - mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); - - // (2) merge each branch to one lanelet - // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::vector> merged_lanelet_with_area; - for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { - const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); - const auto & original = originals.at(i); - double area = 0; - for (const auto & partition : original) { - area += bg::area(partition.polygon2d().basicPolygon()); - } - merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); - } - - // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { - const double length = bg::length(merged_lanelet.centerline()); - const double width = area / length; - for (int i = 0; i < static_cast(width / resolution); ++i) { - const double offset = resolution * i - width / 2; - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); - } - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); - } - return detection_divisions; -} - std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, @@ -1116,7 +350,6 @@ std::optional generateInterpolatedPath( return interpolated_path_info; } -// from here geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) { @@ -1134,609 +367,5 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -static bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data) -{ - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type - } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; // not stop vehicle - } - - // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); - if (is_in_stuck_area && debug_data) { - debug_data->stuck_targets.objects.push_back(object); - return true; - } - } - return false; -} - -static lanelet::LineString3d getLineStringFromArcLength( - const lanelet::ConstLineString3d & linestring, const double s1, const double s2) -{ - lanelet::Points3d points; - double accumulated_length = 0; - size_t start_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s1) { - start_index = i; - break; - } - accumulated_length += length; - } - if (start_index < linestring.size() - 1) { - const auto & p1 = linestring[start_index]; - const auto & p2 = linestring[start_index + 1]; - const double residue = s1 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto start_basic_point = p1.basicPoint() + residue * direction_vector; - const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); - points.push_back(start_point); - } - - accumulated_length = 0; - size_t end_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s2) { - end_index = i; - break; - } - accumulated_length += length; - } - - for (size_t i = start_index + 1; i < end_index; i++) { - const auto p = lanelet::Point3d(linestring[i]); - points.push_back(p); - } - if (end_index < linestring.size() - 1) { - const auto & p1 = linestring[end_index]; - const auto & p2 = linestring[end_index + 1]; - const double residue = s2 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto end_basic_point = p1.basicPoint() + residue * direction_vector; - const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); - points.push_back(end_point); - } - return lanelet::LineString3d{lanelet::InvalId, points}; -} - -static lanelet::ConstLanelet createLaneletFromArcLength( - const lanelet::ConstLanelet & lanelet, const double s1, const double s2) -{ - const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); - // make sure that s1, and s2 are between [0, lane_length] - const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); - const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); - - const auto ratio_s1 = s1_saturated / total_length; - const auto ratio_s2 = s2_saturated / total_length; - - const auto s1_left = - static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s2_left = - static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s1_right = - static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); - const auto s2_right = - static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); - - const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); - const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); - - return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); -} - -bool checkYieldStuckVehicleInIntersection( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, - const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, - DebugData * debug_data) -{ - LineString2d sparse_intersection_path; - const auto [start, end] = interpolated_path_info.lane_id_interval.value(); - for (unsigned i = start; i < end; ++i) { - const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; - const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); - if (turn_direction == "right") { - const double right_x = point.x - width / 2 * std::sin(yaw); - const double right_y = point.y + width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(right_x, right_y); - } else if (turn_direction == "left") { - const double left_x = point.x + width / 2 * std::sin(yaw); - const double left_y = point.y - width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(left_x, left_y); - } else { - // straight - sparse_intersection_path.emplace_back(point.x, point.y); - } - } - lanelet::ConstLanelets yield_stuck_detect_lanelets; - for (const auto & attention_lanelet : attention_lanelets) { - const auto centerline = attention_lanelet.centerline2d().basicLineString(); - std::vector intersects; - bg::intersection(sparse_intersection_path, centerline, intersects); - if (intersects.empty()) { - continue; - } - const auto intersect = intersects.front(); - const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( - centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); - const double yield_stuck_start = - std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); - const double yield_stuck_end = intersect_arc_coords.length; - yield_stuck_detect_lanelets.push_back( - createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); - } - debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { - const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); - - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; - } - for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { - const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); - if (is_in_lanelet) { - debug_data->yield_stuck_targets.objects.push_back(object.object); - return true; - } - } - } - return false; -} - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getLaneletLength3d; - using lanelet::utils::getPolygonFromArcLength; - using lanelet::utils::to2D; - - Polygon2d polygon{}; - if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { - return polygon; - } - - double target_polygon_length = - getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); - lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; - if (path_lanelets.next) { - targets.push_back(path_lanelets.next.value()); - const double next_arc_length = - std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); - target_polygon_length += next_arc_length; - } - const auto target_polygon = - to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); - - if (target_polygon.empty()) { - return polygon; - } - - for (const auto & p : target_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - - polygon.outer().emplace_back(polygon.outer().front()); - bg::correct(polygon); - - return polygon; -} - -std::optional checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double dist_margin, const bool is_parked_vehicle) -{ - for (unsigned i = 0; i < target_lanelets.size(); ++i) { - const auto & ll = target_lanelets.at(i); - if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { - continue; - } - const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); - const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); - if (consider_wrong_direction_vehicle) { - if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - } else { - if (std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is - // positive - if ( - is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return std::make_optional(i); - } - } - } - return std::nullopt; -} - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) -{ - const rclcpp::Time current_time = clock->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(target_objects->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } - } - } - } -} - -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) -{ - const double current_velocity = planner_data->current_velocity->twist.linear.x; - - int assigned_lane_found = false; - - // crop intersection part of the path, and set the reference velocity to intersection_velocity - // for ego's ttc - PathWithLaneId reference_path; - std::optional upstream_stopline{std::nullopt}; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - auto reference_point = path.points.at(i); - // assume backward velocity is current ego velocity - if (i < closest_idx) { - reference_point.point.longitudinal_velocity_mps = current_velocity; - } - if ( - i > last_intersection_stopline_candidate_idx && - std::fabs(reference_point.point.longitudinal_velocity_mps) < - std::numeric_limits::epsilon() && - !upstream_stopline) { - upstream_stopline = i; - } - if (!use_upstream_velocity) { - reference_point.point.longitudinal_velocity_mps = intersection_velocity; - } - reference_path.points.push_back(reference_point); - bool has_objective_lane_id = hasLaneIds(path.points.at(i), associative_ids); - if (assigned_lane_found && !has_objective_lane_id) { - break; - } - assigned_lane_found = has_objective_lane_id; - } - if (!assigned_lane_found) { - return {{0.0, 0.0}}; // has already passed the intersection. - } - - std::vector> original_path_xy; - for (size_t i = 0; i < reference_path.points.size(); ++i) { - const auto & p = reference_path.points.at(i).point.pose.position; - original_path_xy.emplace_back(p.x, p.y); - } - - // apply smoother to reference velocity - PathWithLaneId smoothed_reference_path = reference_path; - if (!smoothPath(reference_path, smoothed_reference_path, planner_data)) { - smoothed_reference_path = reference_path; - } - - // calculate when ego is going to reach each (interpolated) points on the path - TimeDistanceArray time_distance_array{}; - double dist_sum = 0.0; - double passing_time = time_delay; - time_distance_array.emplace_back(passing_time, dist_sum); - - // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so - // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - - const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { - if (upstream_stopline) { - const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, upstream_stopline_point, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - } else { - return std::nullopt; - } - }(); - const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); - const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); - - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { - const auto & p1 = smoothed_reference_path.points.at(i); - const auto & p2 = smoothed_reference_path.points.at(i + 1); - - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); - dist_sum += dist; - - // use average velocity between p1 and p2 - const double average_velocity = - (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - const double passing_velocity = [=]() { - if (use_upstream_velocity) { - if (has_upstream_stopline && i > upstream_stopline_ind) { - return minimum_upstream_velocity; - } - return std::max(average_velocity, minimum_ego_velocity); - } else { - return std::max(average_velocity, minimum_ego_velocity); - } - }(); - passing_time += (dist / passing_velocity); - - time_distance_array.emplace_back(passing_time, dist_sum); - } - debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 5; - debug_ttc_array->layout.dim.at(1).label = "values"; - debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); - debug_ttc_array->data.reserve( - time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); - for (unsigned i = 0; i < time_distance_array.size(); ++i) { - debug_ttc_array->data.push_back(lane_id); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(t); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(d); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.x); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.y); - } - return time_distance_array; -} - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx) -{ - const auto lane_id = assigned_lanelet.id(); - const auto intersection_first_itr = - std::find_if(path.points.cbegin(), path.points.cend(), [&](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end(); - }); - if ( - intersection_first_itr == path.points.begin() || intersection_first_itr == path.points.end()) { - return 0.0; - } - const auto dst_idx = std::distance(path.points.begin(), intersection_first_itr) - 1; - - if (closest_idx > static_cast(dst_idx)) { - return 0.0; - } - - double distance = std::abs(motion_utils::calcSignedArcLength(path.points, closest_idx, dst_idx)); - const auto & lane_first_point = assigned_lanelet.centerline2d().front(); - distance += std::hypot( - path.points.at(dst_idx).point.pose.position.x - lane_first_point.x(), - path.points.at(dst_idx).point.pose.position.y - lane_first_point.y()); - return distance; -} - -void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - is_prioritized_ = is_prioritized; - // find the first conflicting/detection area polygon intersecting the path - if (!first_conflicting_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - conflicting_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_conflicting_lane_ = conflicting_.at(first.value().second); - first_conflicting_area_ = conflicting_area_.at(first.value().second); - } - } - if (!first_attention_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_attention_lane_ = attention_non_preceding_.at(first.value().second); - first_attention_area_ = attention_non_preceding_area_.at(first.value().second); - } - } -} - -static lanelet::ConstLanelets getPrevLanelets( - const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) -{ - lanelet::ConstLanelets previous_lanelets; - for (const auto & ll : lanelets_on_path) { - if (associative_ids.find(ll.id()) != associative_ids.end()) { - return previous_lanelets; - } - previous_lanelets.push_back(ll); - } - return previous_lanelets; -} - -// end inclusive -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - size_t prev_idx = start_idx; - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto & p = path.points.at(i).point.pose; - const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { - continue; - } - prev_idx = i; - const double yaw = tf2::getYaw(p.orientation); - const double x = p.position.x; - const double y = p.position.y; - // NOTE: maybe this is opposite - const double left_x = x + width / 2 * std::sin(yaw); - const double left_y = y - width / 2 * std::cos(yaw); - const double right_x = x - width / 2 * std::sin(yaw); - const double right_y = y + width / 2 * std::cos(yaw); - lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); - rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx, - const double width) -{ - static constexpr double path_lanelet_interval = 1.5; - - const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; - if (!assigned_lane_interval_opt) { - return std::nullopt; - } - const auto assigned_lane_interval = assigned_lane_interval_opt.value(); - const auto & path = interpolated_path_info.path; - - PathLanelets path_lanelets; - // prev - path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids); - path_lanelets.all = path_lanelets.prev; - - // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; - if (closest_idx > assigned_lane_start) { - path_lanelets.all.push_back( - generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); - } - - // ego_or_entry2exit - const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); - path_lanelets.ego_or_entry2exit = - generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); - - // next - if (assigned_lane_end < path.points.size() - 1) { - const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); - const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.next.value()); - } - } - - const auto first_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) - : getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); - const auto last_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) - : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); - if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { - const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); - const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, - path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); - if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); - } - } - return path_lanelets; -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; - } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); -} - } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 33e511d911b82..86a34d1e95114 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -15,12 +15,12 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include "scene_intersection.hpp" #include "util_type.hpp" #include -#include +#include +#include #include #include @@ -35,88 +35,82 @@ namespace behavior_velocity_planner { namespace util { -std::optional insertPoint( + +/** + * @fn + * @brief insert a new pose to the path and return its index + * @return if insertion was successful return the inserted point index + */ +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); +/** + * @fn + * @brief check if a PathPointWithLaneId contains any of the given lane ids + */ bool hasLaneIds( const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); -std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief get objective polygons for detection area + * @fn + * @brief find the first contiguous interval of the path points that contains the specified lane ids + * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is + * found, returns the pair (start-1, end) */ -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle); +std::optional> findLaneIdsInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief Generate a stop line for stuck vehicle - * @param conflicting_areas used to generate stop line for stuck vehicle - * @param original_path ego-car lane - * @param target_path target lane to insert stop point (part of ego-car lane or same to ego-car - * lane) - " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane + * @fn + * @brief return the index of the first point which is inside the given polygon + * @param[in] lane_interval the interval of the path points on the intersection + * @param[in] search_forward flag for search direction */ -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); /** + * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose - * @param path path - * @param closest_idx ego's closest index on the path - * @param current_pose ego's exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); +/** + * @fn + * @brief check if ego is before the target_idx. If the index is same, compare the exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose + * @return true if ego is over the target_idx + */ bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( -lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, -lanelet::ConstLanelet lane); -*/ + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); +/** + * @fn + * @brief check if the given lane has related traffic light + */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos); - -std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds); - +/** + * @fn + * @brief interpolate PathWithLaneId + */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, @@ -125,56 +119,28 @@ std::optional generateInterpolatedPath( geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data); - -bool checkYieldStuckVehicleInIntersection( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, - const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, - DebugData * debug_data); - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); - -std::optional checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double dist_margin, const bool is_parked_vehicle); - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, - const double time_thr); +/** + * @fn + * @brief this function sorts the set of lanelets topologically using topological sort and merges + * the lanelets from each root to each end. each branch is merged and returned with the original + * lanelets + * @param[in] lanelets the set of lanelets + * @param[in] routing_graph_ptr the routing graph + * @return the pair of merged lanelets and their corresponding original lanelets + */ +std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr); -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); - -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval); - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx, - const double width); +/** + * @fn + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygon + */ +std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index d05031bf19436..763d829dacf9b 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -34,178 +34,24 @@ namespace behavior_velocity_planner::util { -struct DebugData -{ - std::optional collision_stop_wall_pose{std::nullopt}; - std::optional occlusion_stop_wall_pose{std::nullopt}; - std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional pass_judge_wall_pose{std::nullopt}; - std::optional> attention_area{std::nullopt}; - std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; - std::optional> adjacent_area{std::nullopt}; - std::optional stuck_vehicle_detect_area{std::nullopt}; - std::optional> yield_stuck_detect_area{std::nullopt}; - std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - std::vector occlusion_polygons; - std::optional> - nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; - std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; -}; - +/** + * @struct + * @brief wrapper class of interpolated path with lane id + */ struct InterpolatedPathInfo { + /** the interpolated path */ autoware_auto_planning_msgs::msg::PathWithLaneId path; + /** discretization interval of interpolation */ double ds{0.0}; + /** the intersection lanelet id */ lanelet::Id lane_id{0}; + /** the associative lane ids of lane_id */ std::set associative_lane_ids{}; + /** the range of indices for the path points with associative lane id */ std::optional> lane_id_interval{std::nullopt}; }; -struct IntersectionLanelets -{ -public: - void update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); - const lanelet::ConstLanelets & attention() const - { - return is_prioritized_ ? attention_non_preceding_ : attention_; - } - const std::vector> & attention_stoplines() const - { - return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; - } - const lanelet::ConstLanelets & conflicting() const { return conflicting_; } - const lanelet::ConstLanelets & adjacent() const { return adjacent_; } - const lanelet::ConstLanelets & occlusion_attention() const - { - return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; - } - const std::vector & attention_area() const - { - return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; - } - const std::vector & conflicting_area() const - { - return conflicting_area_; - } - const std::vector & adjacent_area() const { return adjacent_area_; } - const std::vector & occlusion_attention_area() const - { - return occlusion_attention_area_; - } - const std::optional & first_conflicting_lane() const - { - return first_conflicting_lane_; - } - const std::optional & first_conflicting_area() const - { - return first_conflicting_area_; - } - const std::optional & first_attention_lane() const - { - return first_attention_lane_; - } - const std::optional & first_attention_area() const - { - return first_attention_area_; - } - - lanelet::ConstLanelets attention_; // topologically merged lanelets - std::vector> - attention_stoplines_; // the stop lines for each attention_ lanelets - lanelet::ConstLanelets attention_non_preceding_; - std::vector> - attention_non_preceding_stoplines_; // the stop lines for each attention_non_preceding_ - // lanelets - lanelet::ConstLanelets conflicting_; - lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets - std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets - std::vector attention_area_; // topologically merged lanelets - std::vector attention_non_preceding_area_; - std::vector conflicting_area_; - std::vector adjacent_area_; - std::vector - occlusion_attention_area_; // topologically merged lanelets - // the first area intersecting with the path - // even if lane change/re-routing happened on the intersection, these areas area are supposed to - // be invariant under the 'associative' lanes. - std::optional first_conflicting_lane_{std::nullopt}; - std::optional first_conflicting_area_{std::nullopt}; - std::optional first_attention_lane_{std::nullopt}; - std::optional first_attention_area_{std::nullopt}; - bool is_prioritized_ = false; -}; - -struct IntersectionStopLines -{ - // NOTE: for baselink - size_t closest_idx{0}; - // NOTE: null if path does not conflict with first_conflicting_area - std::optional stuck_stopline{std::nullopt}; - // NOTE: null if path is over map stopline OR its value is calculated negative - std::optional default_stopline{std::nullopt}; - // NOTE: null if the index is calculated negative - std::optional first_attention_stopline{std::nullopt}; - // NOTE: null if footprints do not change from outside to inside of detection area - std::optional occlusion_peeking_stopline{std::nullopt}; - // if the value is calculated negative, its value is 0 - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; -}; - -struct PathLanelets -{ - lanelet::ConstLanelets prev; - // lanelet::ConstLanelet entry2ego; this is included in `all` if exists - lanelet::ConstLanelet - ego_or_entry2exit; // this is `assigned lane` part of the path(not from - // ego) if ego is before the intersection, otherwise from ego to exit - std::optional next = - std::nullopt; // this is nullopt is the goal is inside intersection - lanelet::ConstLanelets all; - lanelet::ConstLanelets - conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with - // conflicting lanelets plus the next lane part of the - // path -}; - -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - -enum class TrafficPrioritizedLevel { - // The target lane's traffic signal is red or the ego's traffic signal has an arrow. - FULLY_PRIORITIZED = 0, - // The target lane's traffic signal is amber - PARTIALLY_PRIORITIZED, - // The target lane's traffic signal is green - NOT_PRIORITIZED -}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index aa51c38b55742..b31506918a2db 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -2,6 +2,7 @@ ros__parameters: forward_path_length: 1000.0 backward_path_length: 5.0 + behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 max_accel: -2.8 max_jerk: -5.0 diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 5da8df9e70c35..8ecad4d9c1548 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -138,6 +138,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Parameters forward_path_length_ = declare_parameter("forward_path_length"); backward_path_length_ = declare_parameter("backward_path_length"); + behavior_output_path_interval_ = declare_parameter("behavior_output_path_interval"); planner_data_.stop_line_extend_length = declare_parameter("stop_line_extend_length"); // nearest search @@ -409,7 +410,8 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + const auto interpolated_path_msg = + interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point output_path_msg = filterStopPathPoint(interpolated_path_msg); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index ded1d08700a1d..a345f1200f721 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -102,6 +102,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // parameter double forward_path_length_; double backward_path_length_; + double behavior_output_path_interval_; // member PlannerData planner_data_; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 591ef8667b81b..667c915ac1d7f 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -28,8 +28,7 @@ bool splineInterpolate( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, - const double interval = 1.0); + const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval); autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( const autoware_auto_planning_msgs::msg::Path & path); autoware_auto_planning_msgs::msg::Path filterStopPathPoint( diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index 39c454e59dcb2..33bb471e5920c 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -33,6 +33,7 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs + traffic_light_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index d2d18d74a5c1a..a17fc43b86f23 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -217,7 +218,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Move to go out state if ego vehicle over deadline. constexpr double signed_deadline_length = -2.0; if (signed_arc_length_to_stop_point < signed_deadline_length) { - RCLCPP_INFO(logger_, "APPROACH -> GO_OUT"); + RCLCPP_DEBUG(logger_, "APPROACH -> GO_OUT"); state_ = State::GO_OUT; stop_signal_received_time_ptr_.reset(); return true; @@ -263,7 +264,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * constexpr double restart_length = 1.0; if (use_initialization_after_start) { if (signed_arc_length_to_stop_point > restart_length) { - RCLCPP_INFO(logger_, "GO_OUT(RESTART) -> APPROACH"); + RCLCPP_DEBUG(logger_, "GO_OUT(RESTART) -> APPROACH"); state_ = State::APPROACH; } } @@ -287,7 +288,7 @@ bool TrafficLightModule::isStopSignal() return true; } - return isTrafficSignalStop(looking_tl_state_); + return traffic_light_utils::isTrafficSignalStop(lane_, looking_tl_state_); } void TrafficLightModule::updateTrafficSignal() @@ -349,36 +350,6 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const } } -bool TrafficLightModule::isTrafficSignalStop( - const autoware_perception_msgs::msg::TrafficSignal & tl_state) const -{ - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { - return false; - } - - const std::string turn_direction = lane_.attributeOr("turn_direction", "else"); - - if (turn_direction == "else") { - return true; - } - if ( - turn_direction == "right" && - hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { - return false; - } - if ( - turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { - return false; - } - if ( - turn_direction == "straight" && - hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { - return false; - } - - return true; -} - bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const { // get traffic signal associated with the regulatory element id @@ -452,25 +423,4 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP return modified_path; } -bool TrafficLightModule::hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const -{ - const auto it_lamp = - std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; - }); - - return it_lamp != tl_state.elements.end(); -} - -bool TrafficLightModule::hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) const -{ - const auto it_lamp = std::find_if( - tl_state.elements.begin(), tl_state.elements.end(), - [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - - return it_lamp != tl_state.elements.end(); -} - } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index c7b2472289bf5..102ddbe2e9fa8 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -88,8 +88,6 @@ class TrafficLightModule : public SceneModuleInterface private: bool isStopSignal(); - bool isTrafficSignalStop(const TrafficSignal & tl_state) const; - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, @@ -97,10 +95,6 @@ class TrafficLightModule : public SceneModuleInterface bool isPassthrough(const double & signed_arc_length) const; - bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) const; - - bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const; - bool findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const; bool isTrafficSignalTimedOut() const; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index ed364f0987f4f..5d1c0c68d8208 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -400,7 +400,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) log_ss << "x: " << point.position.x << " " << "y: " << point.position.y << std::endl; } - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( logger, "start planning route with check points: " << std::endl << log_ss.str()); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index a1536a1bebfbf..9ee874928d7d1 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -144,7 +144,7 @@ void MissionPlanner::checkInitialization() } // All data is ready. Now API is available. - RCLCPP_INFO(get_logger(), "Route API is ready."); + RCLCPP_DEBUG(get_logger(), "Route API is ready."); change_state(RouteState::Message::UNSET); data_check_timer_->cancel(); // stop timer callback } diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index b87cfc7a1e743..edcabec14930d 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -109,8 +109,7 @@ class MPTOptimizer const std::shared_ptr debug_data_ptr, const std::shared_ptr time_keeper_ptr); - std::vector optimizeTrajectory( - const PlannerData & planner_data, const std::vector & smoothed_points); + std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index fe7c416799231..9ca2f31b6ec5a 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -37,6 +37,11 @@ class ObstacleAvoidancePlanner : public rclcpp::Node public: explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); + // NOTE: This is for the static_centerline_optimizer package which utilizes the following + // instance. + std::shared_ptr getMPTOptimizer() const { return mpt_optimizer_ptr_; } + + // private: protected: // for the static_centerline_optimizer package // TODO(murooka) move this node to common class DrivingDirectionChecker diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp index efc75f4c90bba..99c01dcf66960 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp @@ -154,9 +154,6 @@ size_t findEgoSegmentIndex( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points); - std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval); diff --git a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py index daf6beea730d3..1904b45fb8225 100755 --- a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py +++ b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py @@ -49,6 +49,8 @@ def __init__(self, args): self.y_vec.append(deque()) plt.legend(loc="lower left") + plt.xlabel("step [-]") + plt.ylabel("calculation time [ms]") plt.show() def CallbackCalculationCost(self, msg): diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 765136b3cf6f6..7057a2885fd52 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -15,6 +15,7 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" @@ -467,8 +468,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num; } -std::vector MPTOptimizer::optimizeTrajectory( - const PlannerData & planner_data, const std::vector & smoothed_points) +std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); @@ -479,11 +479,11 @@ std::vector MPTOptimizer::optimizeTrajectory( if (prev_optimized_traj_points_ptr_) { return *prev_optimized_traj_points_ptr_; } - return smoothed_points; + return traj_points; }; // 1. calculate reference points - auto ref_points = calcReferencePoints(planner_data, smoothed_points); + auto ref_points = calcReferencePoints(planner_data, traj_points); if (ref_points.size() < 2) { RCLCPP_INFO_EXPRESSION( logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2."); @@ -1519,8 +1519,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( osqp_solver_ptr_->updateCscP(P_csc); osqp_solver_ptr_->updateQ(f); osqp_solver_ptr_->updateCscA(A_csc); - osqp_solver_ptr_->updateL(lower_bound); - osqp_solver_ptr_->updateU(upper_bound); + osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); } else { RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "no warm start"); osqp_solver_ptr_ = std::make_unique( @@ -1709,17 +1708,17 @@ void MPTOptimizer::publishDebugTrajectories( time_keeper_ptr_->tic(__func__); // reference points - const auto ref_traj = trajectory_utils::createTrajectory( - header, trajectory_utils::convertToTrajectoryPoints(ref_points)); + const auto ref_traj = motion_utils::convertToTrajectory( + trajectory_utils::convertToTrajectoryPoints(ref_points), header); debug_ref_traj_pub_->publish(ref_traj); // fixed reference points const auto fixed_traj_points = extractFixedPoints(ref_points); - const auto fixed_traj = trajectory_utils::createTrajectory(header, fixed_traj_points); + const auto fixed_traj = motion_utils::convertToTrajectory(fixed_traj_points, header); debug_fixed_traj_pub_->publish(fixed_traj); // mpt points - const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points); + const auto mpt_traj = motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 6d7367af835b6..23b146a3e5280 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -16,6 +16,7 @@ #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "obstacle_avoidance_planner/debug_marker.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" @@ -236,7 +237,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) "Backward path is NOT supported. Just converting path to trajectory"); const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points); + const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); return; } @@ -268,7 +269,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); } @@ -363,7 +364,7 @@ std::vector ObstacleAvoidancePlanner::optimizeTrajectory( // 2. make trajectory kinematically-feasible and collision-free (= inside the drivable area) // with model predictive trajectory - const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data, p.traj_points); + const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data); time_keeper_ptr_->toc(__func__, " "); return mpt_traj; @@ -656,7 +657,7 @@ void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const // publish trajectories const auto debug_extended_traj = - trajectory_utils::createTrajectory(header, debug_data_ptr_->extended_traj_points); + motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp index b836cee1aeaa6..453d55bd7f770 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -117,15 +117,6 @@ geometry_msgs::msg::Point getNearestPosition( return points.back().pose.position; } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval) { diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ccfd731416475..ee1d97f2c6c01 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -171,15 +171,6 @@ std::vector extendTrajectoryPoints( return output_points; } -Trajectory createTrajectory( - const std::vector & traj_points, const std_msgs::msg::Header & header) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - std::vector getTargetObjectType(rclcpp::Node & node, const std::string & param_prefix) { std::unordered_map types_map{ @@ -525,7 +516,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms publishVelocityLimit(slow_down_vel_limit, "slow_down"); // 7. Publish trajectory - const auto output_traj = createTrajectory(slow_down_traj_points, msg->header); + const auto output_traj = motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); trajectory_pub_->publish(output_traj); // 8. Publish debug data diff --git a/planning/path_smoother/CMakeLists.txt b/planning/path_smoother/CMakeLists.txt index b2fcf9076a7b3..b2f42181f9a0f 100644 --- a/planning/path_smoother/CMakeLists.txt +++ b/planning/path_smoother/CMakeLists.txt @@ -35,3 +35,8 @@ ament_auto_package( launch config ) + +install(PROGRAMS + scripts/calculation_time_plotter.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index bbcc8e7c071f6..364e1e3cc43d8 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -36,7 +36,11 @@ class ElasticBandSmoother : public rclcpp::Node public: explicit ElasticBandSmoother(const rclcpp::NodeOptions & node_options); -protected: + // NOTE: This is for the static_centerline_optimizer package which utilizes the following + // instance. + std::shared_ptr getElasticBandSmoother() const { return eb_path_smoother_ptr_; } + +private: class DrivingDirectionChecker { public: diff --git a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp index a8a97964f21a4..724c1a966781f 100644 --- a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp +++ b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp @@ -113,9 +113,6 @@ size_t findEgoSegmentIndex( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points); - Path create_path(Path path_msg, const std::vector & traj_points); std::vector resampleTrajectoryPoints( diff --git a/planning/path_smoother/scripts/calculation_time_plotter.py b/planning/path_smoother/scripts/calculation_time_plotter.py new file mode 100755 index 0000000000000..3a18fc56f39bc --- /dev/null +++ b/planning/path_smoother/scripts/calculation_time_plotter.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 + +# 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. + +import argparse +from collections import deque + +import matplotlib.pyplot as plt +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import StringStamped + + +class CalculationCostAnalyzer(Node): + def __init__(self, args): + super().__init__("calculation_cost_analyzer") + + self.functions_name = args.functions.split(",") + self.depth = args.depth + + self.calculation_cost_hist = [] + self.sub_calculation_cost = self.create_subscription( + StringStamped, + "/planning/scenario_planning/lane_driving/motion_planning/elastic_band_smoother/debug/calculation_time", + self.CallbackCalculationCost, + 1, + ) + + plt.ion() + plt.figure() + + self.lines = [] + self.y_vec = [] + for f_idx, function_name in enumerate(self.functions_name): + (line,) = plt.plot([], [], label="{}".format(function_name)) + self.lines.append(line) + self.y_vec.append(deque()) + + plt.legend(loc="lower left") + plt.xlabel("step [-]") + plt.ylabel("calculation time [ms]") + plt.show() + + def CallbackCalculationCost(self, msg): + max_y = 0 + max_x = 0 + + for f_idx, function_name in enumerate(self.functions_name): + is_found = False + for line in msg.data.split("\n"): + if function_name in line: + y = float(line.split(":=")[1].split("[ms]")[0]) + self.y_vec[f_idx].append(y) + + is_found = True + break + + if not is_found: + self.y_vec[f_idx].append(None) + + if len(self.y_vec[f_idx]) > self.depth: + self.y_vec[f_idx].popleft() + + x_vec = list(range(len(self.y_vec[f_idx]))) + + valid_x_vec = [] + valid_y_vec = [] + for i in range(len(x_vec)): + if self.y_vec[f_idx][i] is not None: + valid_x_vec.append(x_vec[i]) + valid_y_vec.append(self.y_vec[f_idx][i]) + + self.lines[f_idx].set_xdata(valid_x_vec) + self.lines[f_idx].set_ydata(valid_y_vec) + + if len(valid_y_vec) > 0: + max_x = max(max_x, max(valid_x_vec)) + max_y = max(max_y, max(valid_y_vec)) + + plt.xlim(0, max_x) + plt.ylim(0.0, max_y) + + plt.draw() + plt.pause(0.01) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "-f", + "--functions", + type=str, + default="onPath,calcSmoothedTrajectory", + ) + parser.add_argument( + "-d", + "--depth", + type=float, + default=500, + ) + args = parser.parse_args() + + rclpy.init() + node = CalculationCostAnalyzer(args) + rclpy.spin(node) diff --git a/planning/path_smoother/src/elastic_band.cpp b/planning/path_smoother/src/elastic_band.cpp index ecf847d496f34..8e538a0d6507f 100644 --- a/planning/path_smoother/src/elastic_band.cpp +++ b/planning/path_smoother/src/elastic_band.cpp @@ -14,6 +14,7 @@ #include "path_smoother/elastic_band.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/type_alias.hpp" #include "path_smoother/utils/geometry_utils.hpp" @@ -262,7 +263,7 @@ std::vector EBPathSmoother::smoothTrajectory( // 8. publish eb trajectory const auto eb_traj = - trajectory_utils::createTrajectory(createHeader(clock_.now()), *eb_traj_points); + motion_utils::convertToTrajectory(*eb_traj_points, createHeader(clock_.now())); debug_eb_traj_pub_->publish(eb_traj); time_keeper_ptr_->toc(__func__, " "); @@ -389,7 +390,7 @@ void EBPathSmoother::updateConstraint( // publish fixed trajectory const auto eb_fixed_traj = - trajectory_utils::createTrajectory(createHeader(clock_.now()), debug_fixed_traj_points); + motion_utils::convertToTrajectory(debug_fixed_traj_points, createHeader(clock_.now())); debug_eb_fixed_traj_pub_->publish(eb_fixed_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 3247bfd7d3390..8227882cbb61c 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -15,6 +15,7 @@ #include "path_smoother/elastic_band_smoother.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "path_smoother/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" @@ -167,7 +168,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) "Backward path is NOT supported. Just converting path to trajectory"); const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points); + const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); path_pub_->publish(*path_ptr); return; @@ -220,7 +221,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); const auto output_path_msg = trajectory_utils::create_path(*path_ptr, full_traj_points); path_pub_->publish(output_path_msg); diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index 872fc65a9d456..6409c61700f34 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -34,15 +34,6 @@ namespace path_smoother { namespace trajectory_utils { -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - Path create_path(Path path_msg, const std::vector & traj_points) { path_msg.points.clear(); diff --git a/planning/planning_topic_converter/src/path_to_trajectory.cpp b/planning/planning_topic_converter/src/path_to_trajectory.cpp index f5df79121029b..757dcd0d9f632 100644 --- a/planning/planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/planning_topic_converter/src/path_to_trajectory.cpp @@ -40,15 +40,6 @@ std::vector convertToTrajectoryPoints(const std::vector & trajectory_points) -{ - auto trajectory = motion_utils::convertToTrajectory(trajectory_points); - trajectory.header = header; - - return trajectory; -} } // namespace PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options) @@ -59,7 +50,7 @@ PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options) void PathToTrajectory::process(const Path::ConstSharedPtr msg) { const auto trajectory_points = convertToTrajectoryPoints(msg->points); - const auto output = createTrajectory(msg->header, trajectory_points); + const auto output = motion_utils::convertToTrajectory(trajectory_points, msg->header); pub_->publish(output); } diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp index 3368c49459a55..a4ff1db77e8a1 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -14,11 +14,11 @@ #include "frenet_planner/frenet_planner.hpp" +#include #include #include #include #include -#include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp index c0a3c1d917c25..d848e9b209d28 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp @@ -138,9 +138,6 @@ size_t findEgoSegmentIndex( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points); - std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval); diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index e61f635de67c4..748486edf9ff6 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -16,6 +16,7 @@ #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "path_sampler/path_generation.hpp" #include "path_sampler/prepare_inputs.hpp" #include "path_sampler/utils/geometry_utils.hpp" @@ -244,13 +245,12 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr) if (!generated_traj_points.empty()) { auto full_traj_points = extendTrajectory(planner_data.traj_points, generated_traj_points); const auto output_traj_msg = - trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); } else { auto stopping_traj = trajectory_utils::convertToTrajectoryPoints(planner_data.traj_points); for (auto & p : stopping_traj) p.longitudinal_velocity_mps = 0.0; - const auto output_traj_msg = - trajectory_utils::createTrajectory(path_ptr->header, stopping_traj); + const auto output_traj_msg = motion_utils::convertToTrajectory(stopping_traj, path_ptr->header); traj_pub_->publish(output_traj_msg); } diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index 76fa90c6baa4b..ad516faa717ff 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -58,15 +58,6 @@ void compensateLastPose( } } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval) { diff --git a/planning/static_centerline_optimizer/CMakeLists.txt b/planning/static_centerline_optimizer/CMakeLists.txt index 4731d44ed30bb..a2091dffd061a 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_optimizer/CMakeLists.txt @@ -24,7 +24,6 @@ autoware_package() ament_auto_add_executable(main src/main.cpp src/static_centerline_optimizer_node.cpp - src/successive_trajectory_optimizer_node.cpp src/utils.cpp ) diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp index cfee93fbc0bbe..fa7043de42f5c 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp @@ -52,6 +52,7 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node // plan path std::vector plan_path(const std::vector & route_lane_ids); + std::vector optimize_trajectory(const Path & raw_path) const; void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); @@ -62,6 +63,7 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node const std::string & lanelet2_output_file_path, const std::vector & route_lane_ids, const std::vector & optimized_traj_points); + lanelet::LaneletMapPtr original_map_ptr_{nullptr}; HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp deleted file mode 100644 index 57ebb947b03c2..0000000000000 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/successive_trajectory_optimizer_node.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2022 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. - -// NOTE: This file was copied from a part of implementation in -// https://github.com/autowarefoundation/autoware.universe/blob/main/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp - -#ifndef STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ - -#include "obstacle_avoidance_planner/node.hpp" -#include "static_centerline_optimizer/type_alias.hpp" - -#include -#include -#include -#include - -namespace static_centerline_optimizer -{ -class SuccessiveTrajectoryOptimizer : public obstacle_avoidance_planner::ObstacleAvoidancePlanner -{ -private: - // subscriber - rclcpp::Subscription::SharedPtr centerline_sub_; - -public: - explicit SuccessiveTrajectoryOptimizer(const rclcpp::NodeOptions & node_options); - - // subscriber callback functions - Trajectory on_centerline(const Path & path); -}; -} // namespace static_centerline_optimizer - -#endif // STATIC_CENTERLINE_OPTIMIZER__SUCCESSIVE_TRAJECTORY_OPTIMIZER_NODE_HPP_ diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index f727c0e58d327..f8b96ca0db146 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -17,6 +17,11 @@ + + + + diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 89976be5bdd47..51bd9e87d6ba2 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -28,6 +28,7 @@ motion_utils obstacle_avoidance_planner osqp_interface + path_smoother rclcpp rclcpp_components route_handler diff --git a/planning/static_centerline_optimizer/scripts/tmp/run.sh b/planning/static_centerline_optimizer/scripts/tmp/run.sh new file mode 100755 index 0000000000000..07857a9d923a1 --- /dev/null +++ b/planning/static_centerline_optimizer/scripts/tmp/run.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_background:=false lanelet2_input_file_path:="$HOME/AutonomousDrivingScenarios/map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 0aeec7be0b55d..f2d9dc68a8cdc 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -18,9 +18,11 @@ #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" +#include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" +#include "obstacle_avoidance_planner/node.hpp" +#include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" -#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "static_centerline_optimizer/type_alias.hpp" #include "static_centerline_optimizer/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -62,6 +64,21 @@ Path convert_to_path(const PathWithLaneId & path_with_lane_id) return path; } +Trajectory convert_to_trajectory(const Path & path) +{ + Trajectory traj; + for (const auto & point : path.points) { + TrajectoryPoint traj_point; + traj_point.pose = point.pose; + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + + traj.points.push_back(traj_point); + } + return traj; +} + [[maybe_unused]] lanelet::ConstLanelets get_lanelets_from_route( const RouteHandler & route_handler, const LaneletRoute & route) { @@ -249,14 +266,22 @@ void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_ // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { // load map - lanelet::LaneletMapPtr map_ptr; tier4_map_msgs::msg::MapProjectorInfo map_projector_info; map_projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; - map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + const auto map_ptr = + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); if (!map_ptr) { return nullptr; } + // NOTE: The original map is stored here since the various ids in the lanelet map will change + // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail. + original_map_ptr_ = + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + + // overwrite more dense centerline + lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); + // create map bin msg const auto map_bin_msg = Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); @@ -388,7 +413,7 @@ std::vector StaticCenterlineOptimizerNode::plan_path( const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, route_lane_ids.front()); - // ego nearest search parameters + // get ego nearest search parameters and resample interval in behavior_path_planner const double ego_nearest_dist_threshold = has_parameter("ego_nearest_dist_threshold") ? get_parameter("ego_nearest_dist_threshold").as_double() @@ -397,31 +422,105 @@ std::vector StaticCenterlineOptimizerNode::plan_path( has_parameter("ego_nearest_yaw_threshold") ? get_parameter("ego_nearest_yaw_threshold").as_double() : declare_parameter("ego_nearest_yaw_threshold"); + const double behavior_path_interval = has_parameter("output_path_interval") + ? get_parameter("output_path_interval").as_double() + : declare_parameter("output_path_interval"); // extract path with lane id from lanelets - const auto raw_path_with_lane_id = utils::get_path_with_lane_id( - *route_handler_ptr_, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - + const auto raw_path_with_lane_id = [&]() { + const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( + *route_handler_ptr_, route_lanelets, start_center_pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); + }(); pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); RCLCPP_INFO(get_logger(), "Calculated raw path with lane id and published."); // convert path with lane id to path - const auto raw_path = convert_to_path(raw_path_with_lane_id); + const auto raw_path = [&]() { + const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); + // NOTE: The behavior_velocity_planner resamples with the interval 1.0 somewhere. + return motion_utils::resamplePath(non_resampled_path, 1.0); + }(); pub_raw_path_->publish(raw_path); RCLCPP_INFO(get_logger(), "Converted to path and published."); - // optimize trajectory by the obstacle_avoidance_planner package - SuccessiveTrajectoryOptimizer successive_trajectory_optimizer(create_node_options()); - const auto optimized_traj = successive_trajectory_optimizer.on_centerline(raw_path); - pub_optimized_centerline_->publish(optimized_traj); - const auto optimized_traj_points = motion_utils::convertToTrajectoryPointArray(optimized_traj); - - RCLCPP_INFO(get_logger(), "Optimized trajectory and published."); + // smooth trajectory and road collision avoidance + const auto optimized_traj_points = optimize_trajectory(raw_path); + pub_optimized_centerline_->publish( + motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header)); + RCLCPP_INFO( + get_logger(), "Smoothed trajectory and made it collision free with the road and published."); return optimized_traj_points; } +std::vector StaticCenterlineOptimizerNode::optimize_trajectory( + const Path & raw_path) const +{ + // convert to trajectory points + const auto raw_traj_points = [&]() { + const auto raw_traj = convert_to_trajectory(raw_path); + return motion_utils::convertToTrajectoryPointArray(raw_traj); + }(); + + // create an instance of elastic band and model predictive trajectory. + const auto eb_path_smoother_ptr = + path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); + const auto mpt_optimizer_ptr = + obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer(); + + // NOTE: The optimization is executed every valid_optimized_traj_points_num points. + constexpr int valid_optimized_traj_points_num = 10; + const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; + + // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use + // warm start. + constexpr int num_initial_optimization = 2; + + std::vector whole_optimized_traj_points; + for (int virtual_ego_pose_idx = -num_initial_optimization; + virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { + // calculate virtual ego pose for the optimization + constexpr int virtual_ego_pose_offset_idx = 1; + const auto virtual_ego_pose = + raw_traj_points + .at( + valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + + virtual_ego_pose_offset_idx) + .pose; + + // smooth trajectory by elastic band in the path_smoother package + const auto smoothed_traj_points = + eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); + + // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner + // package + const obstacle_avoidance_planner::PlannerData planner_data{ + raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, + virtual_ego_pose}; + const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); + + // connect the previously and currently optimized trajectory points + for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { + const double dist = tier4_autoware_utils::calcDistance2d( + whole_optimized_traj_points.at(j), optimized_traj_points.front()); + if (dist < 0.5) { + const std::vector extracted_whole_optimized_traj_points{ + whole_optimized_traj_points.begin(), + whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; + whole_optimized_traj_points = extracted_whole_optimized_traj_points; + break; + } + } + for (size_t j = 0; j < optimized_traj_points.size(); ++j) { + whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); + } + } + + return whole_optimized_traj_points; +} + void StaticCenterlineOptimizerNode::on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) { @@ -581,7 +680,7 @@ void StaticCenterlineOptimizerNode::save_map( RCLCPP_INFO(get_logger(), "Updated centerline in map."); // save map with modified center line - lanelet::write(lanelet2_output_file_path, *route_handler_ptr_->getLaneletMapPtr()); + lanelet::write(lanelet2_output_file_path, *original_map_ptr_); RCLCPP_INFO(get_logger(), "Saved map."); } } // namespace static_centerline_optimizer diff --git a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp deleted file mode 100644 index a8663abb2eab7..0000000000000 --- a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2022 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 "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" - -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include -#include -#include -#include -#include -#include - -namespace static_centerline_optimizer -{ -SuccessiveTrajectoryOptimizer::SuccessiveTrajectoryOptimizer( - const rclcpp::NodeOptions & node_options) -: ObstacleAvoidancePlanner(node_options) -{ - // subscriber - centerline_sub_ = create_subscription( - "debug/raw_centerline", rclcpp::QoS{1}.transient_local(), - std::bind(&SuccessiveTrajectoryOptimizer::on_centerline, this, std::placeholders::_1)); - - // update parameters for replan_checker to execute optimization every cycle - std::vector parameters; - parameters.push_back(rclcpp::Parameter("replan.max_path_shape_around_ego_lat_dist", 100.0)); - parameters.push_back(rclcpp::Parameter("replan.max_ego_moving_dist", 100.0)); - parameters.push_back(rclcpp::Parameter("replan.max_goal_moving_dist", 100.0)); - parameters.push_back(rclcpp::Parameter("replan.max_delta_time_sec", 0.0)); - onParam(parameters); -} - -Trajectory SuccessiveTrajectoryOptimizer::on_centerline(const Path & path) -{ - if (path.points.size() < 2) { - RCLCPP_WARN(get_logger(), "Input path size is less than 2."); - return Trajectory{}; - } - - // parameters for input path sampling - const double resample_interval = 2.0; - const double valid_optimized_path_length = 30.0; - const double path_length = motion_utils::calcArcLength(path.points); - const size_t path_segment_num = static_cast(path_length / valid_optimized_path_length); - - const auto resampled_path = motion_utils::resamplePath(path, resample_interval); // TODO(murooka) - const auto resampled_traj_points = - obstacle_avoidance_planner::trajectory_utils::convertToTrajectoryPoints(resampled_path.points); - - const size_t initial_index = 3; - std::vector whole_optimized_traj_points; - - for (size_t i = 0; i < path_segment_num; ++i) { - // calculate initial pose to start optimization - const auto initial_pose = - resampled_path.points.at(initial_index + valid_optimized_path_length / resample_interval * i) - .pose; - - // create planner data - obstacle_avoidance_planner::PlannerData planner_data; - planner_data.traj_points = resampled_traj_points; - planner_data.left_bound = path.left_bound; - planner_data.right_bound = path.right_bound; - planner_data.ego_pose = initial_pose; - - const auto optimized_traj_points = optimizeTrajectory(planner_data); - - for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = tier4_autoware_utils::calcDistance2d( - whole_optimized_traj_points.at(j), optimized_traj_points.front()); - if (dist < 0.5) { - const std::vector extracted_whole_optimized_traj_points{ - whole_optimized_traj_points.begin(), whole_optimized_traj_points.begin() + j - 1}; - whole_optimized_traj_points = extracted_whole_optimized_traj_points; - } - } - - for (size_t j = 0; j < optimized_traj_points.size(); ++j) { - whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); - } - } - - // resample - auto output_traj_msg = motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(whole_optimized_traj_points), 1.0); - output_traj_msg.header = path.header; - - return output_traj_msg; -} -} // namespace static_centerline_optimizer - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(static_centerline_optimizer::SuccessiveTrajectoryOptimizer) diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py index 549ddbd7e5245..141743deb007c 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py +++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py @@ -52,6 +52,14 @@ def generate_test_description(): get_package_share_directory("static_centerline_optimizer"), "config/static_centerline_optimizer.param.yaml", ), + os.path.join( + get_package_share_directory("behavior_path_planner"), + "config/behavior_path_planner.param.yaml", + ), + os.path.join( + get_package_share_directory("path_smoother"), + "config/elastic_band_smoother.param.yaml", + ), os.path.join( get_package_share_directory("obstacle_avoidance_planner"), "config/obstacle_avoidance_planner.param.yaml", diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index 2cc33f9b5f82b..fecd3c2bc8be4 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -21,7 +21,7 @@ #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include +#include #include #include