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 index d8ba7cf19cfee..37d436f9949a2 100644 --- 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 @@ -21,6 +21,9 @@ #include +#include + +#include #include namespace behavior_path_planner::data::lane_change @@ -34,7 +37,17 @@ struct Debug CollisionCheckDebugMap collision_check_objects_after_approval; LaneChangeTargetObjects filtered_objects; geometry_msgs::msg::Polygon execution_area; + geometry_msgs::msg::Pose ego_pose; + lanelet::ConstLanelets current_lanes; + lanelet::ConstLanelets target_lanes; + lanelet::ConstLanelets target_backward_lanes; double collision_check_object_debug_lifetime{0.0}; + double distance_to_end_of_current_lane{std::numeric_limits::max()}; + double distance_to_lane_change_finished{std::numeric_limits::max()}; + double distance_to_abort_finished{std::numeric_limits::max()}; + bool is_able_to_return_to_current_lane{false}; + bool is_stuck{false}; + bool is_abort{false}; void reset() { @@ -44,7 +57,18 @@ struct Debug filtered_objects.current_lane.clear(); filtered_objects.target_lane.clear(); filtered_objects.other_lane.clear(); + execution_area.points.clear(); + current_lanes.clear(); + target_lanes.clear(); + target_backward_lanes.clear(); + collision_check_object_debug_lifetime = 0.0; + distance_to_end_of_current_lane = std::numeric_limits::max(); + distance_to_lane_change_finished = std::numeric_limits::max(); + distance_to_abort_finished = std::numeric_limits::max(); + is_able_to_return_to_current_lane = false; + is_stuck = false; + is_abort = false; } }; } // namespace behavior_path_planner::data::lane_change 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 8863cabdba008..bc4413e1c69d3 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 @@ -20,6 +20,7 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include +#include #include #include @@ -42,7 +43,9 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); -MarkerArray createDebugMarkerArray(const Debug & debug_data); +MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); +MarkerArray createDebugMarkerArray( + const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); } // 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 41d6b7aa6fc19..2958dfd74d992 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -83,15 +83,16 @@ void LaneChangeInterface::updateData() if (isWaitingApproval()) { module_type_->updateLaneChangeStatus(); } - updateDebugMarker(); module_type_->updateSpecialData(); module_type_->resetStopPose(); + updateDebugMarker(); } void LaneChangeInterface::postProcess() { post_process_safety_status_ = module_type_->isApprovedPathSafe(); + updateDebugMarker(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -189,6 +190,7 @@ bool LaneChangeInterface::canTransitSuccessState() auto log_debug_throttled = [&](std::string_view message) -> void { RCLCPP_DEBUG(getLogger(), "%s", message.data()); }; + updateDebugMarker(); if (module_type_->specialExpiredCheck() && isWaitingApproval()) { log_debug_throttled("Run specialExpiredCheck."); @@ -223,6 +225,7 @@ bool LaneChangeInterface::canTransitFailureState() RCLCPP_DEBUG(getLogger(), "%s", message.data()); }; + updateDebugMarker(); log_debug_throttled(__func__); if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { @@ -322,7 +325,7 @@ void LaneChangeInterface::updateDebugMarker() const debug_marker_.markers.clear(); using marker_utils::lane_change_markers::createDebugMarkerArray; - debug_marker_ = createDebugMarkerArray(module_type_->getDebugData()); + debug_marker_ = createDebugMarkerArray(module_type_->getDebugData(), module_type_->getEgoPose()); } 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 c63c9604346d5..80ed9ff9c999f 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -499,15 +499,20 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); + const auto distance_to_lane_change_end = std::invoke([&]() { + auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { - distance_to_end = std::min( - distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); - } + if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { + distance_to_end = std::min( + distance_to_end, + utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); + } + + return std::max(0.0, distance_to_end) - lane_change_buffer; + }); - return (std::max(0.0, distance_to_end) - lane_change_buffer) < threshold; + lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; + return distance_to_lane_change_end < threshold; } bool NormalLaneChange::hasFinishedLaneChange() const @@ -525,6 +530,10 @@ bool NormalLaneChange::hasFinishedLaneChange() const } const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0; + + lane_change_debug_.distance_to_lane_change_finished = + dist_to_lane_change_end + finish_judge_buffer; + if (!reach_lane_change_end) { return false; } @@ -532,22 +541,23 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto arc_length = lanelet::utils::getArcCoordinates(status_.target_lanes, current_pose); const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; - if (!reach_target_lane) { - return false; - } - return true; + lane_change_debug_.distance_to_lane_change_finished = arc_length.distance; + + return reach_target_lane; } bool NormalLaneChange::isAbleToReturnCurrentLane() const { if (status_.lane_change_path.path.points.size() < 2) { + lane_change_debug_.is_able_to_return_to_current_lane = false; return false; } if (!utils::isEgoWithinOriginalLane( status_.current_lanes, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { + lane_change_debug_.is_able_to_return_to_current_lane = false; return false; } @@ -565,12 +575,15 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; - return utils::isEgoWithinOriginalLane( + auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( status_.current_lanes, estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); + lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; + return is_ego_within_original_lane; } } + lane_change_debug_.is_able_to_return_to_current_lane = true; return true; } @@ -611,17 +624,18 @@ bool NormalLaneChange::isAbleToStopSafely() const bool NormalLaneChange::hasFinishedAbort() const { if (!abort_path_) { + lane_change_debug_.is_abort = true; return true; } const auto distance_to_finish = calcSignedArcLength( abort_path_->path.points, getEgoPosition(), abort_path_->info.shift_line.end.position); + lane_change_debug_.distance_to_abort_finished = distance_to_finish; - if (distance_to_finish < 0.0) { - return true; - } + const auto has_finished_abort = distance_to_finish < 0.0; + lane_change_debug_.is_abort = has_finished_abort; - return false; + return has_finished_abort; } bool NormalLaneChange::isAbortState() const @@ -638,6 +652,7 @@ bool NormalLaneChange::isAbortState() const return false; } + lane_change_debug_.is_abort = true; return true; } int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const @@ -792,6 +807,10 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( const auto target_backward_lanes = utils::lane_change::getBackwardLanelets( route_handler, target_lanes, current_pose, backward_length); + lane_change_debug_.current_lanes = current_lanes; + lane_change_debug_.target_lanes = target_lanes; + lane_change_debug_.target_backward_lanes = target_backward_lanes; + // filter objects to get target object index const auto target_obj_index = filterObject(objects, current_lanes, target_lanes, target_backward_lanes); @@ -1840,6 +1859,7 @@ bool NormalLaneChange::isVehicleStuck( bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const { if (current_lanes.empty()) { + lane_change_debug_.is_stuck = false; return false; // can not check } @@ -1859,7 +1879,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); - return isVehicleStuck(current_lanes, detection_distance); + auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + + lane_change_debug_.is_stuck = is_vehicle_stuck; + return is_vehicle_stuck; } void NormalLaneChange::setStopPose(const Pose & stop_pose) 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 67594823eb112..9e44e51d8b72d 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -17,15 +17,18 @@ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include +#include #include #include #include +#include #include #include #include #include +#include #include #include @@ -122,8 +125,39 @@ MarkerArray showFilteredObjects( return marker_array; } -MarkerArray createDebugMarkerArray(const Debug & debug_data) +MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) { + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "execution_info", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::white()); + }; + + MarkerArray marker_array; + + auto safety_check_info_text = default_text_marker(); + safety_check_info_text.pose = ego_pose; + safety_check_info_text.pose.position.z += 4.0; + + std::ostringstream ss; + + ss << "\nDistToEndOfCurrentLane: " << std::setprecision(5) + << debug_data.distance_to_end_of_current_lane + << "\nDistToLaneChangeFinished: " << debug_data.distance_to_lane_change_finished + << (debug_data.is_stuck ? "\nVehicleStuck" : "") + << (debug_data.is_able_to_return_to_current_lane ? "\nAbleToReturnToCurrentLane" : "") + << (debug_data.is_abort ? "\nAborting" : "") + << "\nDistanceToAbortFinished: " << debug_data.distance_to_abort_finished; + + safety_check_info_text.text = ss.str(); + marker_array.markers.push_back(safety_check_info_text); + return marker_array; +} + +MarkerArray createDebugMarkerArray( + const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) +{ + using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::showPolygon; using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; @@ -141,6 +175,20 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data) tier4_autoware_utils::appendMarkerArray(added, &debug_marker); }; + if (!debug_data.execution_area.points.empty()) { + add(createPolygonMarkerArray( + debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); + } + + add(showExecutionInfo(debug_data, ego_pose)); + + // lanes + add(laneletsAsTriangleMarkerArray( + "current_lanes", debug_data.current_lanes, colors::light_yellow(0.2))); + add(laneletsAsTriangleMarkerArray("target_lanes", debug_data.target_lanes, colors::aqua(0.2))); + add(laneletsAsTriangleMarkerArray( + "target_backward_lanes", debug_data.target_backward_lanes, colors::blue(0.2))); + add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); add(showFilteredObjects( debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, @@ -162,11 +210,6 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data) "ego_and_target_polygon_relation_after_approval")); } - if (!debug_data.execution_area.points.empty()) { - add(createPolygonMarkerArray( - debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); - } - return debug_marker; } } // namespace marker_utils::lane_change_markers