refactor(behavior_velocity_walkway_module): add a new module (autowar…

…efoundation#4315) * add behavior_velocity_walkway_module package Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update README Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update behavior_velocity_planner Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tier4 · Jul 19, 2023 · 92b78db · 92b78db
1 parent 8c257b9
commit 92b78db
Show file tree

Hide file tree

Showing 23 changed files with 427 additions and 244 deletions.
diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt
@@ -9,7 +9,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/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/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/...or_velocity_crosswalk_module/src/util.hpp → ...havior_velocity_crosswalk_module/util.hpp b/...or_velocity_crosswalk_module/src/util.hpp → ...havior_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 <boost/assert.hpp>
 #include <boost/assign/list_of.hpp>
@@ -24,6 +24,7 @@
 #include <limits>
 #include <memory>
 #include <optional>
+#include <set>
 #include <string>
 #include <utility>
 #include <vector>
@@ -82,6 +83,20 @@ struct DebugData
   std::vector<std::vector<geometry_msgs::msg::Point>> obj_polygons;
 };
 
+std::vector<lanelet::ConstLanelet> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
+
+std::set<int64_t> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
+
+bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr);
+
 std::vector<geometry_msgs::msg::Point> 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<lanelet::ConstLineString3d> 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
@@ -1,4 +1,3 @@
 <library path="behavior_velocity_crosswalk_module">
   <class type="behavior_velocity_planner::CrosswalkModulePlugin" base_class_type="behavior_velocity_planner::PluginInterface"/>
-  <class type="behavior_velocity_planner::WalkwayModulePlugin" base_class_type="behavior_velocity_planner::PluginInterface"/>
 </library>
diff --git 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 <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/motion_utils.hpp>
@@ -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
@@ -22,66 +22,6 @@
 #include <string>
 #include <vector>
 
-namespace
-{
-std::vector<lanelet::ConstLanelet> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
-{
-  std::vector<lanelet::ConstLanelet> crosswalks;
-
-  // Add current lane id
-  const auto nearest_lane_id =
-    behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose);
-
-  std::vector<int64_t> 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<int64_t> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
-{
-  std::set<int64_t> 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<double>(ns + ".stop_line_distance");
-  wp.stop_duration_sec = node.declare_parameter<double>(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<WalkwayModule>(
-      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<Crosswalk>(
-      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<bool(const std::shared_ptr<SceneModuleInterface> &)>
-WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
-{
-  const auto rh = planner_data_->route_handler_;
-
-  std::set<int64_t> walkway_id_set;
-
-  if (*opt_use_regulatory_element_) {
-    const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
-      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<SceneModuleInterface> & scene_module) {
-    return walkway_id_set.count(scene_module->getModuleId()) == 0;
-  };
-}
 }  // namespace behavior_velocity_planner
 
 #include <pluginlib/class_list_macros.hpp>
 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
@@ -16,7 +16,6 @@
 #define MANAGER_HPP_
 
 #include "scene_crosswalk.hpp"
-#include "scene_walkway.hpp"
 
 #include <behavior_velocity_planner_common/plugin_interface.hpp>
 #include <behavior_velocity_planner_common/plugin_wrapper.hpp>
@@ -56,32 +55,9 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC
   std::optional<bool> 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<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
-    const PathWithLaneId & path) override;
-
-  std::optional<bool> opt_use_regulatory_element_{std::nullopt};
-};
-
 class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>
 {
 };
-
-class WalkwayModulePlugin : public PluginWrapper<WalkwayModuleManager>
-{
-};
-
 }  // 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
@@ -15,7 +15,7 @@
 #ifndef SCENE_CROSSWALK_HPP_
 #define SCENE_CROSSWALK_HPP_
 
-#include "util.hpp"
+#include "behavior_velocity_crosswalk_module/util.hpp"
 
 #include <behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <lanelet2_extension/regulatory_elements/crosswalk.hpp>