From e20d75a202ccce1ce82f02d7f0ed2461291811ba Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 25 Mar 2022 08:35:22 +0900 Subject: [PATCH] feat(behavior_path_planner): add drivable area visualization to observe shared linestring (#499) * feat(behavior_path_planner): visualize drivable areas The visualization visualize lanelet that share same linestrings Signed-off-by: Muhammad Zulfaqar Azmi * Refactor: move function to utilities Signed-off-by: Muhammad Zulfaqar Azmi * Refactor: Remove function and direct call drivable area Signed-off-by: Muhammad Zulfaqar Azmi * feat: parameterize the visualization allows the visualization to be disable in yaml file Signed-off-by: Muhammad Zulfaqar Azmi * feat: change occupancy map to line marker Signed-off-by: Muhammad Zulfaqar Azmi * Feat: working concept, but still need to fix the behind part Signed-off-by: Muhammad Zulfaqar Azmi * ci(pre-commit): autofix * Fix: Linestring direction when appending in debug marker Signed-off-by: Muhammad Zulfaqar Azmi * refactor: some refactoring, and add conditional statement The conditional statements is for empty input Signed-off-by: Muhammad Zulfaqar Azmi * set visualization true by default. And also name changes to fix spelling Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/behavior_path_planner.param.yaml | 2 + .../behavior_path_planner_node.hpp | 2 + .../behavior_path_planner/parameters.hpp | 3 + .../scene_module/avoidance/debug.hpp | 2 + .../behavior_path_planner/utilities.hpp | 2 + .../src/behavior_path_planner_node.cpp | 73 +++++++-------- .../src/scene_module/avoidance/debug.cpp | 93 ++++++++++++++++++- .../behavior_path_planner/src/utilities.cpp | 28 ++++++ 8 files changed, 162 insertions(+), 43 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 7621cbd1c64e5..e809e47b05228 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 769a4569b1d36..34cf77fc37dc5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/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::SharedPtr debug_drivable_area_publisher_; + rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; rclcpp::Publisher::SharedPtr debug_path_publisher_; rclcpp::Publisher::SharedPtr debug_marker_publisher_; void publishDebugMarker(const std::vector & debug_markers); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index c368217184681..9db58a23b70b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/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_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index 26abdd6978620..edfb7d7f84d71 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/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); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 487405eb0c22a..6ddc52727d571 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/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 planner_data); +lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( + const std::shared_ptr & planner_data); // goal management /** diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9248c576cdd0d..0d25f25988aa0 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/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("~/debug/markers", 1); + if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { + debug_drivable_area_lanelets_publisher_ = + create_publisher("~/drivable_area_boundary", 1); + } + // behavior tree manager { - mutex_bt_.lock(); - bt_manager_ = std::make_shared(*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,21 +505,24 @@ 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 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"); @@ -539,11 +530,11 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( - const BehaviorModuleOutput & bt_output, const std::shared_ptr planner_data) + const BehaviorModuleOutput & bt_output) { auto path_candidate = bt_output.path_candidate ? bt_output.path_candidate : std::make_shared(); - 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> & statuses, - const std::shared_ptr planner_data) + const std::vector> & 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 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 lock(mutex_pd_); planner_data_->self_odometry = msg; } void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); planner_data_->dynamic_object = msg; } void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg) { - std::lock_guard 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 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 lock(mutex_pd_); planner_data_->route_handler->setMap(*msg); } void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) { - std::lock_guard 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 diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index 941e4b09fa0a3..d7c3b1325f7dc 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/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; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index a368e7adc9dee..adf78c4992ce5 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1616,6 +1616,34 @@ std::shared_ptr generateCenterLinePath( return centerline_path; } +// TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if +// we can refactor some of the code for better readability +lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( + const std::shared_ptr & planner_data) +{ + const auto & p = planner_data->parameters; + const auto & route_handler = planner_data->route_handler; + const auto & ego_pose = planner_data->self_pose->pose; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("utilities"), + "failed to find closest lanelet within route!!!"); + return {}; + } + + const auto current_lanes = route_handler->getLaneletSequence( + current_lane, ego_pose, p.backward_path_length, p.forward_path_length); + lanelet::ConstLineStrings3d linestring_shared; + for (const auto & lane : current_lanes) { + lanelet::ConstLineStrings3d furthest_line = route_handler->getFurthestLinestring(lane); + linestring_shared.insert(linestring_shared.end(), furthest_line.begin(), furthest_line.end()); + } + + return linestring_shared; +} + PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) { return filterObjectsByVelocity(objects, -lim_v, lim_v);