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"
@@ -158,6 +159,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<MarkerArray>::SharedPtr debug_marker_publisher_;
   void publishDebugMarker(const std::vector<MarkerArray> & debug_markers);

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

@@ -82,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 =
@@ -119,8 +122,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     bt_manager_->registerSceneModule(pull_out_module);
 
     bt_manager_->createBehaviorTree();
-
-    mutex_bt_.unlock();
   }
 
   // turn signal decider
@@ -164,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();
@@ -433,13 +436,10 @@ 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()) {
@@ -451,38 +451,26 @@ void BehaviorPathPlannerNode::waitForData()
       "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_;
-  mutex_pd_.unlock();
+  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
-  mutex_pd_.lock();  // for 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);
@@ -506,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;
     }
@@ -517,33 +505,36 @@ void BehaviorPathPlannerNode::run()
   }
 
   // for remote operation
-  publishModuleStatus(bt_manager_->getModulesStatus(), planner_data);
+  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.",
@@ -552,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") {
@@ -585,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) {
@@ -635,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";
@@ -667,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);
@@ -726,7 +716,6 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
 
   return refined_path;
 }
-
 }  // namespace behavior_path_planner
 
 #include <rclcpp_components/register_node_macro.hpp>

planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp

@@ -557,6 +557,98 @@ MarkerArray createOvehangFurthestLineStringMarkerArray(
 
   return msg;
 }
+
+MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings)
+{
+  const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
+
+  MarkerArray msg;
+  if (linestrings.empty()) {
+    return msg;
+  }
+
+  Marker marker{};
+  marker.header.frame_id = "map";
+  marker.header.stamp = current_time;
+  marker.ns = "shared_linestring_lanelets";
+  marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker.type = Marker::LINE_STRIP;
+  marker.action = Marker::ADD;
+  marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
+  marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.0, 0.0);
+  marker.color = tier4_autoware_utils::createMarkerColor(0.996, 0.658, 0.466, 0.999);
+
+  const auto reserve_size = linestrings.size() / 2;
+  lanelet::ConstLineStrings3d lefts;
+  lanelet::ConstLineStrings3d rights;
+  lefts.reserve(reserve_size);
+  rights.reserve(reserve_size);
+  for (size_t idx = 1; idx < linestrings.size(); idx += 2) {
+    rights.emplace_back(linestrings.at(idx - 1));
+    lefts.emplace_back(linestrings.at(idx));
+  }
+
+  const auto & first_ls = lefts.front().basicLineString();
+  for (const auto & ls : first_ls) {
+    Point p;
+    p.x = ls.x();
+    p.y = ls.y();
+    p.z = ls.z();
+    marker.points.push_back(p);
+  }
+
+  for (auto idx = lefts.cbegin() + 1; idx != lefts.cend(); ++idx) {
+    const auto & marker_back = marker.points.back();
+    Point front;
+    front.x = idx->basicLineString().front().x();
+    front.y = idx->basicLineString().front().y();
+    front.z = idx->basicLineString().front().z();
+    Point front_inverted;
+    front_inverted.x = idx->invert().basicLineString().front().x();
+    front_inverted.y = idx->invert().basicLineString().front().y();
+    front_inverted.z = idx->invert().basicLineString().front().z();
+    const bool isFrontNear = tier4_autoware_utils::calcDistance2d(marker_back, front) <
+                             tier4_autoware_utils::calcDistance2d(marker_back, front_inverted);
+    const auto & left_ls = (isFrontNear) ? idx->basicLineString() : idx->invert().basicLineString();
+    for (auto ls = left_ls.cbegin(); ls != left_ls.cend(); ++ls) {
+      Point p;
+      p.x = ls->x();
+      p.y = ls->y();
+      p.z = ls->z();
+      marker.points.push_back(p);
+    }
+  }
+
+  for (auto idx = rights.crbegin(); idx != rights.crend(); ++idx) {
+    const auto & marker_back = marker.points.back();
+    Point front;
+    front.x = idx->basicLineString().front().x();
+    front.y = idx->basicLineString().front().y();
+    front.z = idx->basicLineString().front().z();
+    Point front_inverted;
+    front_inverted.x = idx->invert().basicLineString().front().x();
+    front_inverted.y = idx->invert().basicLineString().front().y();
+    front_inverted.z = idx->invert().basicLineString().front().z();
+    const bool isFrontFurther = tier4_autoware_utils::calcDistance2d(marker_back, front) >
+                                tier4_autoware_utils::calcDistance2d(marker_back, front_inverted);
+    const auto & right_ls =
+      (isFrontFurther) ? idx->basicLineString() : idx->invert().basicLineString();
+    for (auto ls = right_ls.crbegin(); ls != right_ls.crend(); ++ls) {
+      Point p;
+      p.x = ls->x();
+      p.y = ls->y();
+      p.z = ls->z();
+      marker.points.push_back(p);
+    }
+  }
+
+  if (!marker.points.empty()) {
+    marker.points.push_back(marker.points.front());
+  }
+
+  msg.markers.push_back(marker);
+  return msg;
+}
 }  // namespace marker_utils
 
 std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr)
@@ -593,7 +685,6 @@ std::string toStrInfo(const behavior_path_planner::AvoidPointArray & ap_arr)
   }
   return ss.str();
 }
-
 std::string toStrInfo(const behavior_path_planner::AvoidPoint & ap)
 {
   std::stringstream pids;