diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt similarity index 93% rename from planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt index 8b327291a60f1..4e53e7bd1790b 100644 --- a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_occlusion_spot_module) +project(autoware_behavior_velocity_occlusion_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_occlusion_spot_module/README.md b/planning/autoware_behavior_velocity_occlusion_spot_module/README.md similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/README.md rename to planning/autoware_behavior_velocity_occlusion_spot_module/README.md diff --git a/planning/behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml b/planning/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml rename to planning/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/da.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/da.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml similarity index 90% rename from planning/behavior_velocity_occlusion_spot_module/package.xml rename to planning/autoware_behavior_velocity_occlusion_spot_module/package.xml index ceddd48b045a0..2f56bce7155b5 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_occlusion_spot_module + autoware_behavior_velocity_occlusion_spot_module 0.1.0 - The behavior_velocity_occlusion_spot_module package + The autoware_behavior_velocity_occlusion_spot_module package Taiki Tanaka Tomoya Kimura diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml b/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml new file mode 100644 index 0000000000000..53a2c460c2e44 --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/src/debug.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp similarity index 99% rename from planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 427a6f27be3c4..6ab88baf77b20 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -39,7 +39,7 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) // std::cout << boost::geometry::wkt(line_poly) << std::endl; // std::cout << boost::geometry::wkt(line) << std::endl; - bg::correct(line_poly); + boost::geometry::correct(line_poly); return line_poly; } @@ -142,7 +142,7 @@ Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, cons poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position)); poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position)); - bg::correct(poly); + boost::geometry::correct(poly); return poly; } diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp similarity index 97% rename from planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 5c8d77fc831df..8dc59d07173dc 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -61,6 +61,10 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; using nav_msgs::msg::OccupancyGrid; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + namespace occlusion_cost_value { static constexpr unsigned char FREE_SPACE = 0; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp similarity index 97% rename from planning/behavior_velocity_occlusion_spot_module/src/manager.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index b1757c4142212..cbc7c9ee23b31 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -132,9 +132,10 @@ std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { - return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { - return false; - }; + return + [path]( + [[maybe_unused]] const std::shared_ptr & + scene_module) { return false; }; } } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp similarity index 91% rename from planning/behavior_velocity_occlusion_spot_module/src/manager.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 123d0ef32afc1..4876f325c5213 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -55,11 +55,12 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; -class OcclusionSpotModulePlugin : public PluginWrapper +class OcclusionSpotModulePlugin +: public PluginWrapper { }; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp similarity index 99% rename from planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index ed195023ce67a..f336c7b7db706 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -43,7 +43,8 @@ Polygon2d toFootprintPolygon(const PredictedObject & object, const double scale const Pose & obj_pose = object.kinematics.initial_pose_with_covariance.pose; Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object); // upscale foot print for noise - obj_footprint = upScalePolygon(obj_pose.position, obj_footprint, scale); + obj_footprint = + upScalePolygon(obj_pose.position, obj_footprint, scale); return obj_footprint; } diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp similarity index 96% rename from planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index 9fe06273ea617..819b3b48e5b6a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -47,7 +47,8 @@ void applySafeVelocityConsideringPossibleCollision( // safe slow down: consider ego smooth brake const double v_safe_slow_down = - planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs); + planning_utils::calcDecelerationVelocityFromDistanceToTarget( + j_min, a_min, a0, v0, l_obs); // TODO(tanaka): consider edge case if ego passed safe margin const double v_slow_down = (l_obs < 0 && v0 <= v_safe) ? v_safe : v_safe_slow_down; @@ -62,8 +63,8 @@ void applySafeVelocityConsideringPossibleCollision( safe_velocity = std::max(safe_velocity, v_min); possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity; const auto & pose = possible_collision.collision_with_margin.pose; - const auto & decel_pose = - planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity); + const auto & decel_pose = planning_utils::insertDecelPoint( + pose.position, *inout_path, safe_velocity); if (decel_pose) debug_poses.push_back(decel_pose.value()); } } diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp similarity index 99% rename from planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index e5c5eaec346ff..a466ac2277819 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -61,7 +61,8 @@ namespace autoware::behavior_velocity_planner namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( - const int64_t module_id, const std::shared_ptr & planner_data, + const int64_t module_id, + const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), param_(planner_param) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp similarity index 92% rename from planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 00126a2ebc39a..de39454d08fc7 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -46,14 +46,16 @@ class OcclusionSpotModule : public SceneModuleInterface public: OcclusionSpotModule( - const int64_t module_id, const std::shared_ptr & planner_data, + const int64_t module_id, + const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); /** * @brief plan occlusion spot velocity at unknown area in occupancy grid */ - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + bool modifyPathVelocity( + PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp