From 6d8f026dcfa3e811ebeecd8fa72de9512ac47762 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 29 Jan 2024 12:52:17 +0900 Subject: [PATCH] refactor(lane_change): combine all debug data into single struct (#6173) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * refactor(lane_change): combine all debug data into single struct Signed-off-by: Zulfaqar Azmi * Rename valid path → valid paths Signed-off-by: Zulfaqar Azmi * move the marker creation to marker_utils Signed-off-by: Zulfaqar Azmi * Remove alias Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../interface.hpp | 2 +- .../utils/base_class.hpp | 22 ++------- .../utils/debug_structs.hpp | 49 +++++++++++++++++++ .../utils/markers.hpp | 4 ++ .../src/interface.cpp | 44 ++++------------- .../src/scene.cpp | 40 +++++++-------- .../src/utils/markers.cpp | 44 +++++++++++++++++ 7 files changed, 129 insertions(+), 76 deletions(-) create mode 100644 planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 4e741a2b3db2c..6a9a8c40c01d1 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -128,7 +128,7 @@ class LaneChangeInterface : public SceneModuleInterface bool canTransitIdleToRunningState() override; - void setObjectDebugVisualization() const; + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index e78d706334bae..15fe2f8d2b8d9 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ #include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" @@ -36,7 +37,6 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -137,19 +137,7 @@ class LaneChangeBase const LaneChangeStatus & getLaneChangeStatus() const { return status_; } - const LaneChangePaths & getDebugValidPath() const { return debug_valid_path_; } - - const CollisionCheckDebugMap & getDebugData() const { return object_debug_; } - - const CollisionCheckDebugMap & getAfterApprovalDebugData() const - { - return object_debug_after_approval_; - } - - const LaneChangeTargetObjects & getDebugFilteredObjects() const - { - return debug_filtered_objects_; - } + const data::lane_change::Debug & getDebugData() const { return lane_change_debug_; } const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } @@ -257,12 +245,8 @@ class LaneChangeBase Direction direction_{Direction::NONE}; LaneChangeModuleType type_{LaneChangeModuleType::NORMAL}; - mutable LaneChangePaths debug_valid_path_{}; - mutable CollisionCheckDebugMap object_debug_{}; - mutable CollisionCheckDebugMap object_debug_after_approval_{}; - mutable LaneChangeTargetObjects debug_filtered_objects_{}; - mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; + mutable data::lane_change::Debug lane_change_debug_; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp new file mode 100644 index 0000000000000..8ac1ba3909a50 --- /dev/null +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 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 BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ + +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" + +#include + +#include + +namespace behavior_path_planner::data::lane_change +{ +using utils::path_safety_checker::CollisionCheckDebugMap; +struct Debug +{ + std::string module_type; + LaneChangePaths valid_paths; + CollisionCheckDebugMap collision_check_objects; + CollisionCheckDebugMap collision_check_objects_after_approval; + LaneChangeTargetObjects filtered_objects; + double collision_check_object_debug_lifetime{0.0}; + + void reset() + { + valid_paths.clear(); + collision_check_objects.clear(); + collision_check_objects_after_approval.clear(); + filtered_objects.current_lane.clear(); + filtered_objects.target_lane.clear(); + filtered_objects.other_lane.clear(); + collision_check_object_debug_lifetime = 0.0; + } +}; +} // namespace behavior_path_planner::data::lane_change + +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index d6deecd4f46fa..427a26e9b4295 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -15,9 +15,11 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include #include @@ -26,6 +28,7 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; +using behavior_path_planner::data::lane_change::Debug; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( @@ -37,5 +40,6 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & current_lane_objects, const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); +MarkerArray createDebugMarkerArray(const Debug & debug_data); } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index c8e1229fe5af5..41d6b7aa6fc19 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -83,7 +83,7 @@ void LaneChangeInterface::updateData() if (isWaitingApproval()) { module_type_->updateLaneChangeStatus(); } - setObjectDebugVisualization(); + updateDebugMarker(); module_type_->updateSpecialData(); module_type_->resetStopPose(); @@ -109,7 +109,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() stop_pose_ = module_type_->getStopPose(); - for (const auto & [uuid, data] : module_type_->getAfterApprovalDebugData()) { + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } @@ -133,7 +134,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); - for (const auto & [uuid, data] : module_type_->getDebugData()) { + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } @@ -291,7 +293,7 @@ bool LaneChangeInterface::canTransitFailureState() bool LaneChangeInterface::canTransitIdleToRunningState() { - setObjectDebugVisualization(); + updateDebugMarker(); auto log_debug_throttled = [&](std::string_view message) -> void { RCLCPP_DEBUG(getLogger(), "%s", message.data()); @@ -312,43 +314,15 @@ bool LaneChangeInterface::canTransitIdleToRunningState() return true; } -void LaneChangeInterface::setObjectDebugVisualization() const +void LaneChangeInterface::updateDebugMarker() const { - debug_marker_.markers.clear(); if (!parameters_->publish_debug_marker) { return; } - using marker_utils::showPolygon; - using marker_utils::showPredictedPath; - using marker_utils::showSafetyCheckInfo; - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showFilteredObjects; - - const auto debug_data = module_type_->getDebugData(); - const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); - const auto debug_valid_path = module_type_->getDebugValidPath(); - const auto debug_filtered_objects = module_type_->getDebugFilteredObjects(); debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); - add(showFilteredObjects( - debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, - debug_filtered_objects.other_lane, "object_filtered")); - if (!debug_data.empty()) { - add(showSafetyCheckInfo(debug_data, "object_debug_info")); - add(showPredictedPath(debug_data, "ego_predicted_path")); - add(showPolygon(debug_data, "ego_and_target_polygon_relation")); - } - - if (!debug_after_approval.empty()) { - add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); - add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); - } + using marker_utils::lane_change_markers::createDebugMarkerArray; + debug_marker_ = createDebugMarkerArray(module_type_->getDebugData()); } MarkerArray LaneChangeInterface::getModuleVirtualWall() diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index b115ab7c163c0..b56e9e544ea8d 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -97,7 +97,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) lane_change_parameters_->rss_params_for_stuck, is_stuck); } - debug_valid_path_ = valid_paths; + lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { safe_path.info.current_lanes = current_lanes; @@ -421,13 +421,8 @@ void NormalLaneChange::resetParameters() current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; status_ = {}; + lane_change_debug_.reset(); - object_debug_.clear(); - object_debug_after_approval_.clear(); - debug_filtered_objects_.current_lane.clear(); - debug_filtered_objects_.target_lane.clear(); - debug_filtered_objects_.other_lane.clear(); - debug_valid_path_.clear(); RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } @@ -1112,7 +1107,7 @@ bool NormalLaneChange::getLaneChangePaths( const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const { - object_debug_.clear(); + lane_change_debug_.collision_check_objects.clear(); if (current_lanes.empty() || target_lanes.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; @@ -1158,7 +1153,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto target_objects = getTargetObjects(current_lanes, target_lanes); - debug_filtered_objects_ = target_objects; + lane_change_debug_.filtered_objects = target_objects; const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1385,9 +1380,10 @@ bool NormalLaneChange::getLaneChangePaths( std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && utils::lane_change::passParkedObject( - route_handler, *candidate_path, filtered_objects, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + !is_stuck && + utils::lane_change::passParkedObject( + route_handler, *candidate_path, filtered_objects, lane_change_buffer, is_goal_in_route, + *lane_change_parameters_, lane_change_debug_.collision_check_objects)) { debug_print( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); @@ -1400,7 +1396,8 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, rss_params, is_stuck, object_debug_); + *candidate_path, target_objects, rss_params, is_stuck, + lane_change_debug_.collision_check_objects); if (is_safe) { debug_print("ACCEPT!!!: it is valid and safe!"); @@ -1423,7 +1420,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto & target_lanes = status_.target_lanes; const auto target_objects = getTargetObjects(current_lanes, target_lanes); - debug_filtered_objects_ = target_objects; + lane_change_debug_.filtered_objects = target_objects; CollisionCheckDebugMap debug_data; const bool is_stuck = isVehicleStuck(current_lanes); @@ -1431,16 +1428,17 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { // only for debug purpose - object_debug_.clear(); - object_debug_lifetime_ += (stop_watch_.toc(getModuleTypeStr()) / 1000); - if (object_debug_lifetime_ > 2.0) { + lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.collision_check_object_debug_lifetime += + (stop_watch_.toc(getModuleTypeStr()) / 1000); + if (lane_change_debug_.collision_check_object_debug_lifetime > 2.0) { stop_watch_.toc(getModuleTypeStr(), true); - object_debug_lifetime_ = 0.0; - object_debug_after_approval_.clear(); + lane_change_debug_.collision_check_object_debug_lifetime = 0.0; + lane_change_debug_.collision_check_objects_after_approval.clear(); } if (!safety_status.is_safe) { - object_debug_after_approval_ = debug_data; + lane_change_debug_.collision_check_objects_after_approval = debug_data; } } @@ -1802,7 +1800,7 @@ bool NormalLaneChange::isVehicleStuck( using lanelet::utils::getArcCoordinates; const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; - for (const auto & object : debug_filtered_objects_.current_lane) { + for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point // Note: it needs chattering prevention. diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 719acba405a68..523b770734bc5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -120,4 +121,47 @@ MarkerArray showFilteredObjects( marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); return marker_array; } + +MarkerArray createDebugMarkerArray(const Debug & debug_data) +{ + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; + using marker_utils::lane_change_markers::showAllValidLaneChangePath; + using marker_utils::lane_change_markers::showFilteredObjects; + + const auto & debug_collision_check_object = debug_data.collision_check_objects; + const auto & debug_collision_check_object_after_approval = + debug_data.collision_check_objects_after_approval; + const auto & debug_valid_paths = debug_data.valid_paths; + const auto & debug_filtered_objects = debug_data.filtered_objects; + + MarkerArray debug_marker; + const auto add = [&debug_marker](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker); + }; + + add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); + add(showFilteredObjects( + debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, + debug_filtered_objects.other_lane, "object_filtered")); + + if (!debug_collision_check_object.empty()) { + add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info")); + add(showPredictedPath(debug_collision_check_object, "ego_predicted_path")); + add(showPolygon(debug_collision_check_object, "ego_and_target_polygon_relation")); + } + + if (!debug_collision_check_object_after_approval.empty()) { + add(showSafetyCheckInfo( + debug_collision_check_object_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath( + debug_collision_check_object_after_approval, "ego_predicted_path_after_approval")); + add(showPolygon( + debug_collision_check_object_after_approval, + "ego_and_target_polygon_relation_after_approval")); + } + + return debug_marker; +} } // namespace marker_utils::lane_change_markers