From 07f14cf7c18b4ffc7283d82f20474f64f2ed7d1f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 19 Jul 2023 22:32:06 +0900 Subject: [PATCH] refactor(behavior_velocity_walkway_module): add a new module (#4315) * add behavior_velocity_walkway_module package Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update README Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update behavior_velocity_planner Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 1 - .../util.hpp | 21 ++- .../plugins.xml | 1 - .../src/debug.cpp | 62 -------- .../src/manager.cpp | 142 ------------------ .../src/manager.hpp | 24 --- .../src/scene_crosswalk.hpp | 2 +- .../src/util.cpp | 59 +++++++- planning/behavior_velocity_planner/README.md | 16 ++ .../config/crosswalk.param.yaml | 4 - .../behavior_velocity_planner.launch.xml | 3 + .../behavior_velocity_planner/package.xml | 14 ++ .../CMakeLists.txt | 14 ++ .../README.md | 5 + .../config/walkway.param.yaml | 5 + .../package.xml | 40 +++++ .../plugins.xml | 3 + .../src/debug.cpp | 99 ++++++++++++ .../src/manager.cpp | 113 ++++++++++++++ .../src/manager.hpp | 63 ++++++++ .../src/scene_walkway.cpp | 0 .../src/scene_walkway.hpp | 7 +- 22 files changed, 454 insertions(+), 244 deletions(-) rename planning/behavior_velocity_crosswalk_module/{src => include/behavior_velocity_crosswalk_module}/util.hpp (79%) create mode 100644 planning/behavior_velocity_walkway_module/CMakeLists.txt create mode 100644 planning/behavior_velocity_walkway_module/README.md create mode 100644 planning/behavior_velocity_walkway_module/config/walkway.param.yaml create mode 100644 planning/behavior_velocity_walkway_module/package.xml create mode 100644 planning/behavior_velocity_walkway_module/plugins.xml create mode 100644 planning/behavior_velocity_walkway_module/src/debug.cpp create mode 100644 planning/behavior_velocity_walkway_module/src/manager.cpp create mode 100644 planning/behavior_velocity_walkway_module/src/manager.hpp rename planning/{behavior_velocity_crosswalk_module => behavior_velocity_walkway_module}/src/scene_walkway.cpp (100%) rename planning/{behavior_velocity_crosswalk_module => behavior_velocity_walkway_module}/src/scene_walkway.hpp (93%) diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index d11c7263943af..8f092453b1a67 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -17,7 +17,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp src/manager.cpp src/scene_crosswalk.cpp - src/scene_walkway.cpp src/util.cpp ) diff --git a/planning/behavior_velocity_crosswalk_module/src/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp similarity index 79% rename from planning/behavior_velocity_crosswalk_module/src/util.hpp rename to planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 299eb79906fc2..578ae66e40006 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTIL_HPP_ -#define UTIL_HPP_ +#ifndef BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ +#define BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ #include #include @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,20 @@ struct DebugData std::vector> obj_polygons; }; +std::vector getCrosswalksOnPath( + const geometry_msgs::msg::Pose & current_pose, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs); + +std::set getCrosswalkIdSetOnPath( + const geometry_msgs::msg::Pose & current_pose, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs); + +bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr); + std::vector getPolygonIntersects( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, const geometry_msgs::msg::Point & ego_pos, const size_t max_num); @@ -95,4 +110,4 @@ std::optional getStopLineFromMap( const std::string & attribute_name); } // namespace behavior_velocity_planner -#endif // UTIL_HPP_ +#endif // BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml index 6ab5b00758f29..3d1d720857c17 100644 --- a/planning/behavior_velocity_crosswalk_module/plugins.xml +++ b/planning/behavior_velocity_crosswalk_module/plugins.xml @@ -1,4 +1,3 @@ - diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index aa6acdc8829d5..409898233cbbe 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "scene_crosswalk.hpp" -#include "scene_walkway.hpp" #include #include @@ -175,42 +174,6 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( return msg; } - -visualization_msgs::msg::MarkerArray createWalkwayMarkers( - const DebugData & debug_data, const rclcpp::Time & now, const int64_t module_id) -{ - int32_t uid = planning_utils::bitShift(module_id); - visualization_msgs::msg::MarkerArray msg; - - // Stop point - if (!debug_data.stop_poses.empty()) { - auto marker = createDefaultMarker( - "map", now, "stop point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), - createMarkerColor(1.0, 0.0, 0.0, 0.999)); - for (const auto & p : debug_data.stop_poses) { - marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); - } - msg.markers.push_back(marker); - } - - { - size_t i = 0; - for (const auto & p : debug_data.stop_poses) { - auto marker = createDefaultMarker( - "map", now, "walkway stop judge range", uid + i++, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.1, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.5)); - for (size_t j = 0; j < 50; ++j) { - const auto x = p.position.x + debug_data.stop_judge_range * std::cos(M_PI * 2 / 50 * j); - const auto y = p.position.y + debug_data.stop_judge_range * std::sin(M_PI * 2 / 50 * j); - marker.points.push_back(createPoint(x, y, p.position.z)); - } - marker.points.push_back(marker.points.front()); - msg.markers.push_back(marker); - } - } - - return msg; -} } // namespace motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() @@ -233,21 +196,6 @@ motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() return virtual_walls; } -motion_utils::VirtualWalls WalkwayModule::createVirtualWalls() -{ - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; - wall.text = "walkway"; - wall.ns = std::to_string(module_id_) + "_"; - - wall.style = motion_utils::VirtualWallType::stop; - for (const auto & p : debug_data_.stop_poses) { - wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); - virtual_walls.push_back(wall); - } - return virtual_walls; -} - visualization_msgs::msg::MarkerArray CrosswalkModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; @@ -257,14 +205,4 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createDebugMarkerArray() return debug_marker_array; } - -visualization_msgs::msg::MarkerArray WalkwayModule::createDebugMarkerArray() -{ - visualization_msgs::msg::MarkerArray debug_marker_array; - - appendMarkerArray( - createWalkwayMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); - - return debug_marker_array; -} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 0cf87cade7212..a3f2568b82f90 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -22,66 +22,6 @@ #include #include -namespace -{ -std::vector getCrosswalksOnPath( - const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, - const std::shared_ptr & overall_graphs) -{ - std::vector crosswalks; - - // Add current lane id - const auto nearest_lane_id = - behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); - - std::vector unique_lane_ids; - if (nearest_lane_id) { - // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); - } else { - // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); - } - - for (const auto lane_id : unique_lane_ids) { - const auto ll = lanelet_map->laneletLayer.get(lane_id); - - constexpr int PEDESTRIAN_GRAPH_ID = 1; - const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); - for (const auto & crosswalk : conflicting_crosswalks) { - crosswalks.push_back(crosswalk); - } - } - - return crosswalks; -} - -std::set getCrosswalkIdSetOnPath( - const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, - const std::shared_ptr & overall_graphs) -{ - std::set crosswalk_id_set; - - for (const auto & crosswalk : - getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { - crosswalk_id_set.insert(crosswalk.id()); - } - - return crosswalk_id_set; -} - -bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); - return !lanelet::utils::query::crosswalks(all_lanelets).empty(); -} -} // namespace - namespace behavior_velocity_planner { @@ -204,90 +144,8 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } - -WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()) -{ - const std::string ns(getModuleName()); - - // for walkway parameters - auto & wp = walkway_planner_param_; - wp.stop_line_distance = node.declare_parameter(ns + ".stop_line_distance"); - wp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); -} - -void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) -{ - const auto rh = planner_data_->route_handler_; - if (!opt_use_regulatory_element_) { - opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); - } - - const auto launch = [this, &path](const auto & lanelet) { - const auto attribute = - lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")); - if (attribute != lanelet::AttributeValueString::Walkway) { - return; - } - - if (isModuleRegistered(lanelet.id())) { - return; - } - - const auto & p = walkway_planner_param_; - const auto logger = logger_.get_child("walkway_module"); - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - - registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); - }; - - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - - for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->crosswalkLanelet()); - } - } else { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); - - for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk); - } - } -} - -std::function &)> -WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) -{ - const auto rh = planner_data_->route_handler_; - - std::set walkway_id_set; - - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - - for (const auto & crosswalk : crosswalk_leg_elem_map) { - walkway_id_set.insert(crosswalk.first->id()); - } - } else { - walkway_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); - } - - return [walkway_id_set](const std::shared_ptr & scene_module) { - return walkway_id_set.count(scene_module->getModuleId()) == 0; - }; -} } // namespace behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( behavior_velocity_planner::CrosswalkModulePlugin, behavior_velocity_planner::PluginInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::WalkwayModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index ed342400808f1..481a246af80dd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -16,7 +16,6 @@ #define MANAGER_HPP_ #include "scene_crosswalk.hpp" -#include "scene_walkway.hpp" #include #include @@ -56,32 +55,9 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC std::optional opt_use_regulatory_element_{std::nullopt}; }; -class WalkwayModuleManager : public SceneModuleManagerInterface -{ -public: - explicit WalkwayModuleManager(rclcpp::Node & node); - - const char * getModuleName() override { return "walkway"; } - -private: - WalkwayModule::PlannerParam walkway_planner_param_{}; - - void launchNewModules(const PathWithLaneId & path) override; - - std::function &)> getModuleExpiredFunction( - const PathWithLaneId & path) override; - - std::optional opt_use_regulatory_element_{std::nullopt}; -}; - class CrosswalkModulePlugin : public PluginWrapper { }; - -class WalkwayModulePlugin : public PluginWrapper -{ -}; - } // namespace behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 327ec693cb264..6a26cf4f96e29 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -15,7 +15,7 @@ #ifndef SCENE_CROSSWALK_HPP_ #define SCENE_CROSSWALK_HPP_ -#include "util.hpp" +#include "behavior_velocity_crosswalk_module/util.hpp" #include #include diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 4868a06dc909d..72c091cd8bc85 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "util.hpp" +#include "behavior_velocity_crosswalk_module/util.hpp" #include #include @@ -48,6 +48,63 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::Line2d; using tier4_autoware_utils::Point2d; +std::vector getCrosswalksOnPath( + const geometry_msgs::msg::Pose & current_pose, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs) +{ + std::vector crosswalks; + + // Add current lane id + const auto nearest_lane_id = + behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + // Add subsequent lane_ids from nearest lane_id + unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); + } else { + // Add all lane_ids in path + unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + } + + for (const auto lane_id : unique_lane_ids) { + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); + for (const auto & crosswalk : conflicting_crosswalks) { + crosswalks.push_back(crosswalk); + } + } + + return crosswalks; +} + +std::set getCrosswalkIdSetOnPath( + const geometry_msgs::msg::Pose & current_pose, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs) +{ + std::set crosswalk_id_set; + + for (const auto & crosswalk : + getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { + crosswalk_id_set.insert(crosswalk.id()); + } + + return crosswalk_id_set; +} + +bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + return !lanelet::utils::query::crosswalks(all_lanelets).empty(); +} + std::vector getPolygonIntersects( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, const geometry_msgs::msg::Point & ego_pos, diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index 8ddc9ac94426a..4fc5b0a047e2f 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -19,8 +19,24 @@ It consists of several modules. Please refer to the links listed below for detai - [No Stopping Area](no-stopping-area-design.md) - [Run Out](run-out-design.md) - [Speed Bump](speed-bump-design.md) + - [Out of Lane](out-of-lane-design.md) +- [Blind Spot](../behavior_velocity_blind_spot_module/README.md) +- [Crosswalk](../behavior_velocity_crosswalk_module/README.md) +- [Walkway](../behavior_velocity_walkway_module/README.md) +- [Detection Area](../behavior_velocity_detection_area_module/README.md) +- [Intersection](../behavior_velocity_intersection_module/README.md) +- [MergeFromPrivate](../behavior_velocity_intersection_module/README.md#merge-from-private) +- [Stop Line](../behavior_velocity_stop_line_module/README.md) +- [Virtual Traffic Light](../behavior_velocity_virtual_traffic_light_module/README.md) +- [Traffic Light](../behavior_velocity_traffic_light_module/README.md) +- [Occlusion Spot](../behavior_velocity_occlusion_spot_module/README.md) +- [No Stopping Area](../behavior_velocity_no_stopping_area_module/README.md) +- [Run Out](../behavior_velocity_run_out_module/README.md) +- [Speed Bump](../behavior_velocity_speed_bump_module/README.md) +- [Out of Lane](../behavior_velocity_out_of_lane_module/README.md) + When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose. So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position. diff --git a/planning/behavior_velocity_planner/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/config/crosswalk.param.yaml index b20c83b5fd65a..8a4b80a44d84d 100644 --- a/planning/behavior_velocity_planner/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_planner/config/crosswalk.param.yaml @@ -39,7 +39,3 @@ bicycle: true # [-] whether to look and stop by BICYCLE objects motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects - - walkway: - stop_duration_sec: 1.0 # [s] stop time at stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index c08cfcbfa32f2..7afa8378e28bf 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -10,6 +10,7 @@ + @@ -29,6 +30,7 @@ + @@ -55,6 +57,7 @@ + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 0f131eebcd40c..5a307398e9afd 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -63,6 +63,20 @@ ament_cmake_ros ament_lint_auto autoware_lint_common + behavior_velocity_blind_spot_module + behavior_velocity_crosswalk_module + behavior_velocity_detection_area_module + behavior_velocity_intersection_module + behavior_velocity_no_drivable_lane_module + behavior_velocity_no_stopping_area_module + behavior_velocity_occlusion_spot_module + behavior_velocity_out_of_lane_module + behavior_velocity_run_out_module + behavior_velocity_speed_bump_module + behavior_velocity_stop_line_module + behavior_velocity_traffic_light_module + behavior_velocity_virtual_traffic_light_module + behavior_velocity_walkway_module ament_cmake diff --git a/planning/behavior_velocity_walkway_module/CMakeLists.txt b/planning/behavior_velocity_walkway_module/CMakeLists.txt new file mode 100644 index 0000000000000..cd41dec991bc6 --- /dev/null +++ b/planning/behavior_velocity_walkway_module/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_walkway_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene_walkway.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_walkway_module/README.md b/planning/behavior_velocity_walkway_module/README.md new file mode 100644 index 0000000000000..d4abe170b2bf7 --- /dev/null +++ b/planning/behavior_velocity_walkway_module/README.md @@ -0,0 +1,5 @@ +## Walkway + +### Role + +This module decide to stop before the ego will cross the walkway including crosswalk to enter or exit the private area. diff --git a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml new file mode 100644 index 0000000000000..b9f278796b8ae --- /dev/null +++ b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + walkway: + stop_duration_sec: 1.0 # [s] stop time at stop position + stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/planning/behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_walkway_module/package.xml new file mode 100644 index 0000000000000..2c72959d6b1d4 --- /dev/null +++ b/planning/behavior_velocity_walkway_module/package.xml @@ -0,0 +1,40 @@ + + + + behavior_velocity_walkway_module + 0.1.0 + The behavior_velocity_walkway_module package + + Satoshi Ota + Tomoya Kimura + Shumpei Wakabayashi + Takayuki Murooka + + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + + autoware_auto_planning_msgs + behavior_velocity_crosswalk_module + behavior_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_walkway_module/plugins.xml b/planning/behavior_velocity_walkway_module/plugins.xml new file mode 100644 index 0000000000000..971a49f2cb044 --- /dev/null +++ b/planning/behavior_velocity_walkway_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_walkway_module/src/debug.cpp b/planning/behavior_velocity_walkway_module/src/debug.cpp new file mode 100644 index 0000000000000..51db3c204f2a4 --- /dev/null +++ b/planning/behavior_velocity_walkway_module/src/debug.cpp @@ -0,0 +1,99 @@ +// Copyright 2020 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 "scene_walkway.hpp" + +#include +#include +#include + +#include + +namespace behavior_velocity_planner +{ + +using motion_utils::createSlowDownVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using visualization_msgs::msg::Marker; + +namespace +{ +visualization_msgs::msg::MarkerArray createWalkwayMarkers( + const DebugData & debug_data, const rclcpp::Time & now, const int64_t module_id) +{ + int32_t uid = planning_utils::bitShift(module_id); + visualization_msgs::msg::MarkerArray msg; + + // Stop point + if (!debug_data.stop_poses.empty()) { + auto marker = createDefaultMarker( + "map", now, "stop point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), + createMarkerColor(1.0, 0.0, 0.0, 0.999)); + for (const auto & p : debug_data.stop_poses) { + marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); + } + msg.markers.push_back(marker); + } + + { + size_t i = 0; + for (const auto & p : debug_data.stop_poses) { + auto marker = createDefaultMarker( + "map", now, "walkway stop judge range", uid + i++, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.1, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.5)); + for (size_t j = 0; j < 50; ++j) { + const auto x = p.position.x + debug_data.stop_judge_range * std::cos(M_PI * 2 / 50 * j); + const auto y = p.position.y + debug_data.stop_judge_range * std::sin(M_PI * 2 / 50 * j); + marker.points.push_back(createPoint(x, y, p.position.z)); + } + marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + } + + return msg; +} +} // namespace + +motion_utils::VirtualWalls WalkwayModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; + wall.text = "walkway"; + wall.ns = std::to_string(module_id_) + "_"; + + wall.style = motion_utils::VirtualWallType::stop; + for (const auto & p : debug_data_.stop_poses) { + wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + virtual_walls.push_back(wall); + } + return virtual_walls; +} + +visualization_msgs::msg::MarkerArray WalkwayModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + appendMarkerArray( + createWalkwayMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); + + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp new file mode 100644 index 0000000000000..f2e844063568a --- /dev/null +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -0,0 +1,113 @@ +// Copyright 2020 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 "manager.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ + +using lanelet::autoware::Crosswalk; + +WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + + // for walkway parameters + auto & wp = walkway_planner_param_; + wp.stop_line_distance = node.declare_parameter(ns + ".stop_line_distance"); + wp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); +} + +void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) +{ + const auto rh = planner_data_->route_handler_; + if (!opt_use_regulatory_element_) { + opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); + } + + const auto launch = [this, &path](const auto & lanelet) { + const auto attribute = + lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")); + if (attribute != lanelet::AttributeValueString::Walkway) { + return; + } + + if (isModuleRegistered(lanelet.id())) { + return; + } + + const auto & p = walkway_planner_param_; + const auto logger = logger_.get_child("walkway_module"); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + + registerModule(std::make_shared( + lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); + }; + + if (*opt_use_regulatory_element_) { + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + launch(crosswalk.first->crosswalkLanelet()); + } + } else { + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), + rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk); + } + } +} + +std::function &)> +WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) +{ + const auto rh = planner_data_->route_handler_; + + std::set walkway_id_set; + + if (*opt_use_regulatory_element_) { + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + walkway_id_set.insert(crosswalk.first->id()); + } + } else { + walkway_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), + rh->getOverallGraphPtr()); + } + + return [walkway_id_set](const std::shared_ptr & scene_module) { + return walkway_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::WalkwayModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp new file mode 100644 index 0000000000000..555e3a73db103 --- /dev/null +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -0,0 +1,63 @@ +// Copyright 2020 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene_walkway.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ + +using autoware_auto_planning_msgs::msg::PathWithLaneId; + +class WalkwayModuleManager : public SceneModuleManagerInterface +{ +public: + explicit WalkwayModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "walkway"; } + +private: + WalkwayModule::PlannerParam walkway_planner_param_{}; + + void launchNewModules(const PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const PathWithLaneId & path) override; + + std::optional opt_use_regulatory_element_{std::nullopt}; +}; + +class WalkwayModulePlugin : public PluginWrapper +{ +}; + +} // namespace behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp b/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp similarity index 100% rename from planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp rename to planning/behavior_velocity_walkway_module/src/scene_walkway.cpp diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp b/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp similarity index 93% rename from planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp rename to planning/behavior_velocity_walkway_module/src/scene_walkway.hpp index 8aebcbf2f347f..5580da39f2546 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -15,16 +15,13 @@ #ifndef SCENE_WALKWAY_HPP_ #define SCENE_WALKWAY_HPP_ -#include "scene_crosswalk.hpp" -#include "util.hpp" +#include "behavior_velocity_crosswalk_module/util.hpp" +#include "scene_walkway.hpp" #include #include #include -#include -#include - #include #include #include