diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 6a0c61963ac81..8cd84cfbed663 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -14,8 +14,10 @@ #include "footprint.hpp" +#include #include +#include #include #include @@ -28,6 +30,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop { +using Point2d = tier4_autoware_utils::Point2d; tier4_autoware_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, const PlannerParam & params, const double hysteresis) @@ -58,6 +61,44 @@ tier4_autoware_utils::Polygon2d make_forward_footprint( {}}; } +//create footprint using predicted_path of object +tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y) +{ + tier4_autoware_utils::Polygon2d translated_polygon; + const boost::geometry::strategy::transform::translate_transformer translation(x, y); + boost::geometry::transform(polygon, translated_polygon, translation); + return translated_polygon; +} + +tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d& base_footprint) +{ + const auto angle = tf2::getYaw(pose.orientation); + return translate_polygon( + tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); +} + +tier4_autoware_utils::MultiPolygon2d create_object_footprints( + const std::vector & objects, const PlannerParam & params) +{ + tier4_autoware_utils::MultiPolygon2d footprints; + + for (const auto & object : objects) { + const auto front = object.shape.dimensions.x / 2 + params.extra_object_width; + const auto rear = -object.shape.dimensions.x / 2 - params.extra_object_width; + const auto left = object.shape.dimensions.y / 2 + params.extra_object_width; + const auto right = -object.shape.dimensions.y / 2 - params.extra_object_width; + tier4_autoware_utils::Polygon2d base_footprint; + base_footprint.outer() = { + Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left}, + Point2d{front, left}}; + for (const auto & path : object.kinematics.predicted_paths) + for (const auto & pose : path.path) + footprints.push_back(create_footprint(pose, base_footprint)); + } + + return footprints; +} + tier4_autoware_utils::Polygon2d project_to_pose( const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) { diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index c28e89ac6c9f6..bf98545f5dc0b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include +#include #include @@ -40,6 +41,10 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( tier4_autoware_utils::Polygon2d make_forward_footprint( const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis); +tier4_autoware_utils::MultiPolygon2d create_object_footprints( + const std::vector & objects, const PlannerParam & params); +tier4_autoware_utils::Polygon2d translate_polygon(const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y); +tier4_autoware_utils::Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d& base_footprint); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project /// @param [in] pose projection pose