feat(behavior_path_planner): add drivable area visualization to obser…

…ve shared linestring (tier4#499)

* feat(behavior_path_planner): visualize drivable areas

The visualization visualize lanelet that share same linestrings

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Refactor: move function to utilities

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Refactor: Remove function and direct call drivable area

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* feat: parameterize the visualization

allows the visualization to be disable in yaml file

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* feat: change occupancy map to line marker

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Feat: working concept, but still need to fix the behind part

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* ci(pre-commit): autofix

* Fix: Linestring direction when appending in debug marker

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor: some refactoring, and add conditional statement

The conditional statements is for empty input

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* set visualization true by default.

And also name changes to fix spelling

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -16,3 +16,5 @@
 
     refine_goal_search_radius_range: 7.5
     intersection_search_distance: 30.0
+
+    visualize_drivable_area_for_shared_linestrings_lanelet: true

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/behavior_tree_manager.hpp"
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
+#include "behavior_path_planner/scene_module/avoidance/debug.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
 #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
@@ -164,6 +165,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
 private:
   rclcpp::Publisher<OccupancyGrid>::SharedPtr debug_drivable_area_publisher_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_drivable_area_lanelets_publisher_;
   rclcpp::Publisher<Path>::SharedPtr debug_path_publisher_;
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -48,6 +48,9 @@ struct BehaviorPathPlannerParameters
   double right_over_hang;
   double base_link2front;
   double base_link2rear;
+
+  // drivable area visualization
+  bool visualize_drivable_area_for_shared_linestrings_lanelet;
 };
 
 #endif  // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp

@@ -96,6 +96,8 @@ MarkerArray makeOverhangToRoadShoulderMarkerArray(
 MarkerArray createOvehangFurthestLineStringMarkerArray(
   const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r,
   const double g, const double b);
+
+MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings);
 }  // namespace marker_utils
 
 std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr);

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -251,6 +251,8 @@ OccupancyGrid generateDrivableArea(
   const lanelet::ConstLanelets & lanes, const double resolution, const double vehicle_length,
   const std::shared_ptr<const PlannerData> planner_data);
 
+lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets(
+  const std::shared_ptr<const PlannerData> & planner_data);
 // goal management
 
 /**

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -30,20 +30,6 @@
 #include <utility>
 #include <vector>
 
-namespace
-{
-rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr)
-{
-  rclcpp::CallbackGroup::SharedPtr callback_group =
-    node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-
-  auto sub_opt = rclcpp::SubscriptionOptions();
-  sub_opt.callback_group = callback_group;
-
-  return sub_opt;
-}
-}  // namespace
-
 namespace behavior_path_planner
 {
 using tier4_planning_msgs::msg::PathChangeModuleId;
@@ -62,26 +48,21 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   }
 
   velocity_subscriber_ = create_subscription<Odometry>(
-    "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1),
-    createSubscriptionOptions(this));
+    "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1));
   perception_subscriber_ = create_subscription<PredictedObjects>(
-    "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1),
-    createSubscriptionOptions(this));
+    "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1));
   external_approval_subscriber_ = create_subscription<ApprovalMsg>(
     "~/input/external_approval", 1,
-    std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1),
-    createSubscriptionOptions(this));
+    std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1));
   force_approval_subscriber_ = create_subscription<PathChangeModule>(
-    "~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1),
-    createSubscriptionOptions(this));
+    "~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1));
 
   // route_handler
   vector_map_subscriber_ = create_subscription<HADMapBin>(
     "~/input/vector_map", rclcpp::QoS{1}.transient_local(),
-    std::bind(&BehaviorPathPlannerNode::onMap, this, _1), createSubscriptionOptions(this));
+    std::bind(&BehaviorPathPlannerNode::onMap, this, _1));
   route_subscriber_ = create_subscription<HADMapRoute>(
-    "~/input/route", 1, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1),
-    createSubscriptionOptions(this));
+    "~/input/route", 1, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1));
 
   // publisher
   path_publisher_ = create_publisher<PathWithLaneId>("~/output/path", 1);
@@ -91,8 +72,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
   debug_drivable_area_publisher_ = create_publisher<OccupancyGrid>("~/debug/drivable_area", 1);
   debug_path_publisher_ = create_publisher<Path>("~/debug/path_for_visualize", 1);
-  debug_avoidance_msg_array_publisher_ =
-    create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
 
   // For remote operation
   plan_ready_publisher_ = create_publisher<PathChangeModule>("~/output/ready", 1);
@@ -103,10 +82,13 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   // Debug
   debug_marker_publisher_ = create_publisher<MarkerArray>("~/debug/markers", 1);
 
+  if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
+    debug_drivable_area_lanelets_publisher_ =
+      create_publisher<MarkerArray>("~/drivable_area_boundary", 1);
+  }
+
   // behavior tree manager
   {
-    mutex_bt_.lock();
-
     bt_manager_ = std::make_shared<BehaviorTreeManager>(*this, getBehaviorTreeManagerParam());
 
     auto side_shift_module =
@@ -140,8 +122,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     bt_manager_->registerSceneModule(pull_out_module);
 
     bt_manager_->createBehaviorTree();
-
-    mutex_bt_.unlock();
   }
 
   // turn signal decider
@@ -185,6 +165,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.turn_light_on_threshold_dis_lat = declare_parameter("turn_light_on_threshold_dis_lat", 0.3);
   p.turn_light_on_threshold_dis_long = declare_parameter("turn_light_on_threshold_dis_long", 10.0);
   p.turn_light_on_threshold_time = declare_parameter("turn_light_on_threshold_time", 3.0);
+  p.visualize_drivable_area_for_shared_linestrings_lanelet =
+    declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true);
 
   // vehicle info
   const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
@@ -454,56 +436,41 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam()
 void BehaviorPathPlannerNode::waitForData()
 {
   // wait until mandatory data is ready
-  mutex_pd_.lock();  // for planner_data_
   while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) {
-    mutex_pd_.unlock();
     RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route to be ready");
     rclcpp::spin_some(this->get_node_base_interface());
     rclcpp::Rate(100).sleep();
-    mutex_pd_.lock();
   }
 
   while (rclcpp::ok()) {
     if (planner_data_->dynamic_object && planner_data_->self_odometry) {
       break;
     }
-
-    mutex_pd_.unlock();
     RCLCPP_WARN_THROTTLE(
       get_logger(), *get_clock(), 5000,
       "waiting for vehicle pose, vehicle_velocity, and obstacles");
     rclcpp::spin_some(this->get_node_base_interface());
     rclcpp::Rate(100).sleep();
-    mutex_pd_.lock();
   }
 
   self_pose_listener_.waitForFirstPose();
   planner_data_->self_pose = self_pose_listener_.getCurrentPose();
-  mutex_pd_.unlock();
 }
 
 void BehaviorPathPlannerNode::run()
 {
   RCLCPP_DEBUG(get_logger(), "----- BehaviorPathPlannerNode start -----");
-  mutex_bt_.lock();  // for bt_manager_
-  mutex_pd_.lock();  // for planner_data_
 
   // update planner data
-  planner_data_->self_pose = self_pose_listener_.getCurrentPose();
-
-  // NOTE: planner_data must not be referenced for multithreading
-  const auto planner_data = planner_data_;
+  updateCurrentPose();
 
   // run behavior planner
-  const auto output = bt_manager_->run(planner_data);
+  const auto output = bt_manager_->run(planner_data_);
 
   // path handling
-  const auto path = getPath(output, planner_data);
-  const auto path_candidate = getPathCandidate(output, planner_data);
-
-  // update planner data
+  const auto path = getPath(output);
+  const auto path_candidate = getPathCandidate(output);
   planner_data_->prev_output_path = path;
-  mutex_pd_.unlock();
 
   auto clipped_path = modifyPathForSmoothGoalConnection(*path);
   clipPathLength(clipped_path);
@@ -527,7 +494,7 @@ void BehaviorPathPlannerNode::run()
       hazard_signal.command = output.turn_signal_info.hazard_signal.command;
     } else {
       turn_signal = turn_signal_decider_.getTurnSignal(
-        *path, planner_data->self_pose->pose, *(planner_data->route_handler),
+        *path, planner_data_->self_pose->pose, *(planner_data_->route_handler),
         output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance);
       hazard_signal.command = HazardLightsCommand::DISABLE;
     }
@@ -538,33 +505,36 @@ void BehaviorPathPlannerNode::run()
   }
 
   // for remote operation
-  publishModuleStatus(bt_manager_->getModulesStatus(), planner_data);
-  debug_avoidance_msg_array_publisher_->publish(bt_manager_->getAvoidanceDebugMsgArray());
+  publishModuleStatus(bt_manager_->getModulesStatus());
+
   publishDebugMarker(bt_manager_->getDebugMarkers());
 
-  mutex_bt_.unlock();
+  if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
+    const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray(
+      util::getDrivableAreaForAllSharedLinestringLanelets(planner_data_));
+    debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines);
+  }
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
-PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
-  const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
+PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output)
 {
   // TODO(Horibe) do some error handling when path is not available.
 
-  auto path = bt_output.path ? bt_output.path : planner_data->prev_output_path;
-  path->header = planner_data->route_handler->getRouteHeader();
+  auto path = bt_output.path ? bt_output.path : planner_data_->prev_output_path;
+  path->header = planner_data_->route_handler->getRouteHeader();
   path->header.stamp = this->now();
   RCLCPP_DEBUG(
     get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND");
   return path;
 }
 
 PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
-  const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
+  const BehaviorModuleOutput & bt_output)
 {
   auto path_candidate =
     bt_output.path_candidate ? bt_output.path_candidate : std::make_shared<PathWithLaneId>();
-  path_candidate->header = planner_data->route_handler->getRouteHeader();
+  path_candidate->header = planner_data_->route_handler->getRouteHeader();
   path_candidate->header.stamp = this->now();
   RCLCPP_DEBUG(
     get_logger(), "BehaviorTreeManager: path candidate is %s.",
@@ -573,8 +543,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
 }
 
 void BehaviorPathPlannerNode::publishModuleStatus(
-  const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses,
-  const std::shared_ptr<PlannerData> planner_data)
+  const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses)
 {
   auto getModuleType = [](std::string name) {
     if (name == "LaneChange") {
@@ -606,7 +575,7 @@ void BehaviorPathPlannerNode::publishModuleStatus(
       running_modules.modules.push_back(module);
     }
     if (status->module_name == "LaneChange") {
-      const auto force_approval = planner_data->approval.is_force_approved;
+      const auto force_approval = planner_data_->approval.is_force_approved;
       if (
         force_approval.module_name == "ForceLaneChange" &&
         (now - force_approval.stamp).seconds() < 0.5) {
@@ -656,26 +625,28 @@ void BehaviorPathPlannerNode::publishDebugMarker(const std::vector<MarkerArray>
   debug_marker_publisher_->publish(msg);
 }
 
+void BehaviorPathPlannerNode::updateCurrentPose()
+{
+  auto self_pose = self_pose_listener_.getCurrentPose();
+  planner_data_->self_pose = self_pose;
+}
+
 void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->self_odometry = msg;
 }
 void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->dynamic_object = msg;
 }
 void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->approval.is_approved.data = msg->approval;
   // TODO(wep21): Replace msg stamp after {stamp: now} is implemented in ros2 topic pub
   planner_data_->approval.is_approved.stamp = this->now();
 }
 void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
   auto getModuleName = [](PathChangeModuleId module) {
     if (module.type == PathChangeModuleId::FORCE_LANE_CHANGE) {
       return "ForceLaneChange";
@@ -688,12 +659,10 @@ void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstShare
 }
 void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->route_handler->setMap(*msg);
 }
 void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
   const bool is_first_time = !(planner_data_->route_handler->isHandlerReady());
 
   planner_data_->route_handler->setRoute(*msg);
@@ -747,7 +716,6 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
 
   return refined_path;
 }
-
 }  // namespace behavior_path_planner
 
 #include <rclcpp_components/register_node_macro.hpp>