From 0dab5389f255742303e442e71104438660a59e7a Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 21 Jun 2022 13:50:40 +0900 Subject: [PATCH] feat(behavior_path_planner): lane change revised visualization Signed-off-by: Muhammad Zulfaqar Azmi --- planning/behavior_path_planner/CMakeLists.txt | 1 + .../config/lane_change/lane_change.param.yaml | 1 + .../behavior_path_planner/debug_utilities.hpp | 38 ++- .../scene_module/lane_change/debug.hpp | 42 ++++ .../lane_change/lane_change_module.hpp | 8 + .../scene_module/lane_change/util.hpp | 8 +- .../scene_module/scene_module_interface.hpp | 2 +- .../behavior_path_planner/utilities.hpp | 11 +- .../src/behavior_path_planner_node.cpp | 1 + .../src/debug_utilities.cpp | 2 + .../src/scene_module/lane_change/debug.cpp | 216 ++++++++++++++++++ .../lane_change/lane_change_module.cpp | 38 ++- .../src/scene_module/lane_change/util.cpp | 42 +++- .../behavior_path_planner/src/utilities.cpp | 48 +++- 14 files changed, 434 insertions(+), 24 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index e2437247f8da0..be5e3d3d4d64a 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -23,6 +23,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/lane_following/lane_following_module.cpp src/scene_module/lane_change/lane_change_module.cpp src/scene_module/lane_change/util.cpp + src/scene_module/lane_change/debug.cpp src/scene_module/pull_over/pull_over_module.cpp src/scene_module/pull_over/util.cpp src/scene_module/pull_out/pull_out_module.cpp diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 810fa2561e14d..5267d2d8b3cf2 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -24,3 +24,4 @@ lateral_distance_threshold: 5.0 expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 + publish_debug_marker: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp index 8edf9e8941cd9..f06b1fb293a0c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp @@ -16,11 +16,12 @@ #include "behavior_path_planner/path_shifter/path_shifter.hpp" #include "behavior_path_planner/path_utilities.hpp" -#include "behavior_path_planner/utilities.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include +#include #include #include #include @@ -35,16 +36,51 @@ namespace marker_utils { using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::ShiftPointArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +struct CollisionCheckDebug +{ + std::string failed_reason; + std::size_t lane_id; + Pose current_pose; + Twist current_twist; + Pose expected_ego_pose; + Pose expected_obj_pose; + Pose relative_to_ego; + bool is_front; + bool allow_lane_change; + std::vector lerped_path; + std::vector ego_predicted_path; + Polygon2d ego_polygon; +}; + +constexpr std::array, 10> colorsList() +{ + constexpr std::array red = {1., 0., 0.}; + constexpr std::array green = {0., 1., 0.}; + constexpr std::array blue = {0., 0., 1.}; + constexpr std::array yellow = {1., 1., 0.}; + constexpr std::array aqua = {0., 1., 1.}; + constexpr std::array magenta = {1., 0., 1.}; + constexpr std::array medium_orchid = {0.729, 0.333, 0.827}; + constexpr std::array light_pink = {1, 0.713, 0.756}; + constexpr std::array light_yellow = {1, 1, 0.878}; + constexpr std::array light_steel_blue = {0.690, 0.768, 0.870}; + return {red, green, blue, yellow, aqua, + magenta, medium_orchid, light_pink, light_yellow, light_steel_blue}; +} + inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } MarkerArray createPoseMarkerArray( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp new file mode 100644 index 0000000000000..a36962fa87b81 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp @@ -0,0 +1,42 @@ +// Copyright 2022 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_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ + +#include "behavior_path_planner/debug_utilities.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" + +#include +#include +#include + +namespace marker_utils::lane_change_markers +{ +using behavior_path_planner::LaneChangePath; +using marker_utils::CollisionCheckDebug; +using visualization_msgs::msg::MarkerArray; +MarkerArray showObjectInfo( + const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showAllLaneChangeLanes(const std::vector & lanes, std::string && ns); +MarkerArray showLerpedPose( + const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showEgoPredictedPaths( + const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showEgoPolygon( + const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showPolygonPose( + const std::unordered_map & obj_debug_vec, std::string && ns); +} // namespace marker_utils::lane_change_markers +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 5fafac1264ce6..aa9103a067b7d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ +#include "behavior_path_planner/scene_module/lane_change/debug.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utilities.hpp" @@ -31,12 +32,14 @@ #include #include +#include #include #include namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using marker_utils::CollisionCheckDebug; struct LaneChangeParameters { @@ -66,6 +69,7 @@ struct LaneChangeParameters double lateral_distance_threshold; double expected_front_deceleration; double expected_rear_deceleration; + bool publish_debug_marker; }; struct LaneChangeStatus @@ -181,6 +185,10 @@ class LaneChangeModule : public SceneModuleInterface bool isCurrentSpeedLow() const; bool isAbortConditionSatisfied() const; bool hasFinishedLaneChange() const; + + void setObjectDebugVisualization() const; + mutable std::unordered_map object_debug_; + mutable std::vector candidate_path_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 2c56de0faacf9..b9f1276ceb35d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -29,6 +29,8 @@ #include #include +#include +#include #include namespace behavior_path_planner::lane_change_utils @@ -61,13 +63,15 @@ bool selectSafePath( const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const double & vehicle_width, const double & vehicle_length, const behavior_path_planner::LaneChangeParameters & ros_parameters, - LaneChangePath * selected_path); + LaneChangePath * selected_path, + std::unordered_map & debug_data); bool isLaneChangePathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const double & vehicle_width, const double & vehicle_length, - const behavior_path_planner::LaneChangeParameters & ros_parameters, const bool use_buffer = true, + const behavior_path_planner::LaneChangeParameters & ros_parameters, + std::unordered_map & debug_data, const bool use_buffer = true, const double acceleration = 0.0); bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 4ec7448733652..261c4b14e4546 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -241,7 +241,7 @@ class SceneModuleInterface rclcpp::Logger logger_; protected: - MarkerArray debug_marker_; + mutable MarkerArray debug_marker_; rclcpp::Clock::SharedPtr clock_; mutable AvoidanceDebugMsgArray::SharedPtr debug_avoidance_msg_array_ptr_{}; 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 e89e86439eae5..c98f8a941e3c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/debug_utilities.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" @@ -126,6 +127,7 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; namespace bg = boost::geometry; using geometry_msgs::msg::Pose; +using marker_utils::CollisionCheckDebug; struct FrenetCoordinate3d { @@ -359,6 +361,8 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const Shape & obj_shape); +std::string getUuidStr(const PredictedObject & obj); + std::vector getPredictedPathFromObj( const PredictedObject & obj, const bool & is_use_all_predicted_path); @@ -397,7 +401,7 @@ bool isLongitudinalDistanceEnough( bool hasEnoughDistance( const Pose & expected_ego_pose, const Twist & ego_current_twist, const Pose & expected_object_pose, const Twist & object_current_twist, - const CollisionCheckParameters & param); + const CollisionCheckParameters & param, CollisionCheckDebug & debug); bool isLateralDistanceEnough( const double & relative_lateral_distance, const double & lateral_distance_threshold); @@ -407,14 +411,15 @@ bool isSafeInLaneletCollisionCheck( const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, const double & check_time_resolution, const PredictedObject & target_object, - const PredictedPath & target_object_path, const CollisionCheckParameters & check_param); + const PredictedPath & target_object_path, const CollisionCheckParameters & check_param, + CollisionCheckDebug & debug); bool isSafeInFreeSpaceCollisionCheck( const Pose & ego_current_pose, const Twist & ego_current_twist, const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, const double & check_time_resolution, const PredictedObject & target_object, - const CollisionCheckParameters & check_param); + const CollisionCheckParameters & check_param, CollisionCheckDebug & debug); } // namespace util } // namespace behavior_path_planner 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 29bc6beee8432..ab4b1db35deca 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -340,6 +340,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lateral_distance_threshold = dp("lateral_distance_threshold", 5.0); p.expected_front_deceleration = dp("expected_front_deceleration", -1.0); p.expected_rear_deceleration = dp("expected_rear_deceleration", -1.0); + p.publish_debug_marker = dp("publish_debug_marker", false); // validation of parameters if (p.lane_change_sampling_num < 1) { diff --git a/planning/behavior_path_planner/src/debug_utilities.cpp b/planning/behavior_path_planner/src/debug_utilities.cpp index 99d32d251c74e..4be131906d143 100644 --- a/planning/behavior_path_planner/src/debug_utilities.cpp +++ b/planning/behavior_path_planner/src/debug_utilities.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/debug_utilities.hpp" +#include "behavior_path_planner/utilities.hpp" + namespace marker_utils { using behavior_path_planner::ShiftPoint; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp new file mode 100644 index 0000000000000..741ad3b93439a --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp @@ -0,0 +1,216 @@ +// Copyright 2022 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. + +#include "behavior_path_planner/path_shifter/path_shifter.hpp" + +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace marker_utils::lane_change_markers +{ +using geometry_msgs::msg::Point; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; + +MarkerArray showObjectInfo( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0.0, 1.0, 1.0, 0.999)); + + MarkerArray marker_array; + int32_t id{0}; + + marker_array.markers.reserve(obj_debug_vec.size()); + + for (const auto & [uuid, info] : obj_debug_vec) { + marker.id = ++id; + marker.pose = info.current_pose; + + std::ostringstream ss; + + ss << info.failed_reason << "\nLon: " << std::setprecision(4) << info.relative_to_ego.position.x + << "\nLat: " << info.relative_to_ego.position.y; + + marker.text = ss.str(); + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +MarkerArray showAllLaneChangeLanes(const std::vector & lanes, std::string && ns) +{ + if (lanes.empty()) { + return MarkerArray{}; + } + + MarkerArray marker_array; + int32_t id{0}; + + constexpr auto colors = colorsList(); + const auto loop_size = std::min(lanes.size(), colors.size()); + + marker_array.markers.reserve(loop_size); + + for (std::size_t idx = 0; idx < loop_size; ++idx) { + const auto & color = colors.at(idx); + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.1, 0.0), createMarkerColor(color[0], color[1], color[2], 0.9)); + + marker.points.reserve(lanes.at(idx).path.points.size()); + + for (const auto & point : lanes.at(idx).path.points) { + marker.points.push_back(point.point.pose.position); + } + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +MarkerArray showLerpedPose( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + MarkerArray marker_array; + int32_t id{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::POINTS, + createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(1.0, 0.3, 1.0, 0.9)); + + marker.points.reserve(info.lerped_path.size()); + + for (const auto & point : info.lerped_path) { + marker.points.push_back(point.position); + } + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +MarkerArray showEgoPredictedPaths( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + MarkerArray marker_array; + constexpr auto colors = colorsList(); + + constexpr float scale_val = 0.2; + + int32_t id{0}; + for (const auto & [uuid, info] : obj_debug_vec) { + const auto loop_size = std::min(info.ego_predicted_path.size(), colors.size()); + + for (std::size_t idx = 0; idx < loop_size; ++idx) { + const auto & path = info.ego_predicted_path.at(idx).path; + const auto & color = colors.at(idx); + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::LINE_STRIP, + createMarkerScale(scale_val, scale_val, scale_val), + createMarkerColor(color[0], color[1], color[2], 0.9)); + + marker.points.reserve(path.size()); + + for (const auto & point : path) { + marker.points.push_back(point.position); + } + + marker_array.markers.push_back(marker); + } + } + return marker_array; +} + +MarkerArray showEgoPolygon( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + constexpr float scale_val = 0.2; + int32_t id{0}; + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(scale_val, scale_val, scale_val), createMarkerColor(1.0, 1.0, 1.0, 0.9)); + + MarkerArray marker_array; + marker_array.markers.reserve(obj_debug_vec.size()); + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto & ego_polygon = info.ego_polygon.outer(); + + if (ego_polygon.empty()) { + continue; + } + + marker.id = ++id; + marker.points.reserve(ego_polygon.size()); + + for (const auto & p : ego_polygon) { + marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); + } + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +MarkerArray showPolygonPose( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + constexpr auto colors = colorsList(); + const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); + MarkerArray marker_array; + int32_t id{0}; + size_t idx{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto & color = colors.at(idx); + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::POINTS, + createMarkerScale(0.2, 0.2, 0.2), createMarkerColor(color[0], color[1], color[2], 0.999)); + marker.points.reserve(2); + marker.points.push_back(info.expected_ego_pose.position); + marker.points.push_back(info.expected_obj_pose.position); + marker_array.markers.push_back(marker); + ++idx; + if (idx >= loop_size) { + break; + } + } + + return marker_array; +} +} // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index de7804ef4e505..dc5e9337f7b7f 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -77,6 +77,7 @@ void LaneChangeModule::onExit() { clearWaitingApproval(); removeRTCStatus(); + debug_marker_.markers.clear(); current_state_ = BT::NodeStatus::IDLE; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); } @@ -193,9 +194,6 @@ CandidateOutput LaneChangeModule::planCandidate() const if (selected_path.path.points.empty()) { return output; } - // if (selected_path.path.points.empty()) { - // return CandidateOutput{}; - // } const auto start_idx = selected_path.shift_point.start_idx; const auto end_idx = selected_path.shift_point.end_idx; @@ -392,12 +390,21 @@ std::pair LaneChangeModule::getSafePath( if (valid_paths.empty()) { return std::make_pair(false, false); } + candidate_path_ = valid_paths; // select safe path + object_debug_.clear(); bool found_safe_path = lane_change_utils::selectSafePath( valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose, current_twist, common_parameters.vehicle_width, common_parameters.vehicle_length, parameters_, - &safe_path); + &safe_path, object_debug_); + + if (parameters_.publish_debug_marker) { + setObjectDebugVisualization(); + } else { + debug_marker_.markers.clear(); + } + return std::make_pair(true, found_safe_path); } @@ -463,10 +470,11 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto check_lanes = route_handler->getCheckTargetLanesFromPath( path.path, status_.lane_change_lanes, check_distance_with_path); + std::unordered_map debug_data; is_path_safe = lane_change_utils::isLaneChangePathSafe( path.path, current_lanes, check_lanes, objects, current_pose, current_twist, - common_parameters.vehicle_width, common_parameters.vehicle_length, parameters_, false, - status_.lane_change_path.acceleration); + common_parameters.vehicle_width, common_parameters.vehicle_length, parameters_, debug_data, + false, status_.lane_change_path.acceleration); } // check vehicle velocity thresh @@ -533,4 +541,22 @@ bool LaneChangeModule::hasFinishedLaneChange() const return travel_distance > finish_distance; } +void LaneChangeModule::setObjectDebugVisualization() const +{ + using marker_utils::lane_change_markers::showEgoPolygon; + using marker_utils::lane_change_markers::showLerpedPose; + using marker_utils::lane_change_markers::showObjectInfo; + using marker_utils::lane_change_markers::showPolygonPose; + + debug_marker_.markers.clear(); + const auto add = [this](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + }; + + add(showObjectInfo(object_debug_, "object_debug_info")); + add(showLerpedPose(object_debug_, "lerp_pose_before_true")); + add(showPolygonPose(object_debug_, "expected_pose")); + add(showEgoPolygon(object_debug_, "ego_lerped_polygon")); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 5c5afbdb76457..b0dda7912c0ee 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -274,12 +274,13 @@ bool selectSafePath( const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const double & vehicle_width, const double & vehicle_length, - const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path) + const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path, + std::unordered_map & debug_data) { for (const auto & path : paths) { if (isLaneChangePathSafe( path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_twist, - vehicle_width, vehicle_length, ros_parameters, true, path.acceleration)) { + vehicle_width, vehicle_length, ros_parameters, debug_data, true, path.acceleration)) { *selected_path = path; return true; } @@ -331,7 +332,9 @@ bool isLaneChangePathSafe( const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const double & vehicle_width, const double & vehicle_length, - const LaneChangeParameters & ros_parameters, const bool use_buffer, const double acceleration) + const LaneChangeParameters & ros_parameters, + std::unordered_map & debug_data, const bool use_buffer, + const double acceleration) { if (dynamic_objects == nullptr) { return true; @@ -379,15 +382,37 @@ bool isLaneChangePathSafe( check_param.rear_vehicle_reaction_time = ros_parameters.rear_vehicle_reaction_time; check_param.safety_time_margin_for_control = ros_parameters.safety_time_margin_for_control; + const auto assignDebugData = [](const PredictedObject & obj) { + CollisionCheckDebug debug; + const auto key = util::getUuidStr(obj); + debug.current_pose = obj.kinematics.initial_pose_with_covariance.pose; + debug.current_twist = obj.kinematics.initial_twist_with_covariance.twist; + return std::make_pair(key, debug); + }; + + const auto appendDebugInfo = + [&debug_data](std::pair & obj, bool && is_allowed) { + const auto & key = obj.first; + auto & element = obj.second; + element.allow_lane_change = is_allowed; + if (debug_data.find(key) != debug_data.end()) { + debug_data[key] = element; + } else { + debug_data.insert(obj); + } + }; + for (const auto & i : current_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); + auto current_debug_data = assignDebugData(obj); const auto predicted_paths = util::getPredictedPathFromObj(obj, ros_parameters.use_all_predicted_path); for (const auto & obj_path : predicted_paths) { if (!util::isSafeInLaneletCollisionCheck( current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, current_lane_check_start_time, current_lane_check_end_time, time_resolution, obj, - obj_path, check_param)) { + obj_path, check_param, current_debug_data.second)) { + appendDebugInfo(current_debug_data, false); return false; } } @@ -396,6 +421,8 @@ bool isLaneChangePathSafe( // Collision check for objects in lane change target lane for (const auto & i : target_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); + auto current_debug_data = assignDebugData(obj); + current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path); bool is_object_in_target = false; if (ros_parameters.use_predicted_path_outside_lanelet) { is_object_in_target = true; @@ -415,7 +442,8 @@ bool isLaneChangePathSafe( if (!util::isSafeInLaneletCollisionCheck( current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, - obj_path, check_param)) { + obj_path, check_param, current_debug_data.second)) { + appendDebugInfo(current_debug_data, false); return false; } } @@ -423,10 +451,12 @@ bool isLaneChangePathSafe( if (!util::isSafeInFreeSpaceCollisionCheck( current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, - check_param)) { + check_param, current_debug_data.second)) { + appendDebugInfo(current_debug_data, false); return false; } } + appendDebugInfo(current_debug_data, true); } return true; } diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 740574bb34b3a..90e5485f1bcc7 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1856,6 +1856,15 @@ Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const return object_polygon; } +std::string getUuidStr(const PredictedObject & obj) +{ + std::stringstream hex_value; + for (const auto & uuid : obj.object_id.uuid) { + hex_value << std::hex << std::setfill('0') << std::setw(2) << +uuid; + } + return hex_value.str(); +} + template ProjectedDistancePoint pointToSegment( const Point2d & reference_point, const Point2d & point_from_ego, @@ -2051,10 +2060,11 @@ bool isLongitudinalDistanceEnough( bool hasEnoughDistance( const Pose & expected_ego_pose, const Twist & ego_current_twist, const Pose & expected_object_pose, const Twist & object_current_twist, - const CollisionCheckParameters & param) + const CollisionCheckParameters & param, CollisionCheckDebug & debug) { const auto front_vehicle_pose = projectCurrentPoseToTarget(expected_ego_pose, expected_object_pose); + debug.relative_to_ego = front_vehicle_pose; if (isLateralDistanceEnough( front_vehicle_pose.position.y, param.lateral_distance_max_threshold)) { @@ -2062,6 +2072,7 @@ bool hasEnoughDistance( } const auto is_obj_in_front = isObjectFront(front_vehicle_pose); + debug.is_front = is_obj_in_front; const auto front_vehicle_velocity = (is_obj_in_front) ? object_current_twist.linear : ego_current_twist.linear; @@ -2094,13 +2105,20 @@ bool isSafeInLaneletCollisionCheck( const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, const double & check_time_resolution, const PredictedObject & target_object, - const PredictedPath & target_object_path, const CollisionCheckParameters & check_param) + const PredictedPath & target_object_path, const CollisionCheckParameters & check_param, + CollisionCheckDebug & debug) { + const auto lerp_path_reserve = (check_end_time - check_start_time) / check_time_resolution; + if (lerp_path_reserve > 1e-3) { + debug.lerped_path.reserve(static_cast(lerp_path_reserve)); + } + for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { Pose expected_obj_pose; tier4_autoware_utils::Polygon2d obj_polygon; if (!util::getObjectExpectedPoseAndConvertToPolygon( target_object_path, target_object, expected_obj_pose, obj_polygon, t)) { + debug.failed_reason = "get_obj_info_failed"; continue; } @@ -2109,22 +2127,32 @@ bool isSafeInLaneletCollisionCheck( if (!util::getEgoExpectedPoseAndConvertToPolygon( ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, ego_vehicle_length, ego_vehicle_width)) { + debug.failed_reason = "get_ego_info_failed"; continue; } + debug.ego_polygon = ego_polygon; if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + debug.failed_reason = "overlap_polygon"; return false; } + debug.lerped_path.push_back(expected_ego_pose); + util::getProjectedDistancePointFromPolygons( ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); + debug.expected_ego_pose = expected_ego_pose; + debug.expected_obj_pose = expected_obj_pose; const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; if (!util::hasEnoughDistance( - expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, check_param)) { + expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, check_param, + debug)) { + debug.failed_reason = "not_enough_longitudinal"; return false; } } + debug.failed_reason = ""; return true; } @@ -2133,7 +2161,7 @@ bool isSafeInFreeSpaceCollisionCheck( const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, const double & check_time_resolution, const PredictedObject & target_object, - const CollisionCheckParameters & check_param) + const CollisionCheckParameters & check_param, CollisionCheckDebug & debug) { tier4_autoware_utils::Polygon2d obj_polygon; if (!util::calcObjectPolygon(target_object, &obj_polygon)) { @@ -2145,10 +2173,13 @@ bool isSafeInFreeSpaceCollisionCheck( if (!util::getEgoExpectedPoseAndConvertToPolygon( ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, ego_vehicle_length, ego_vehicle_width)) { + debug.failed_reason = "get_ego_info_failed"; continue; } + debug.ego_polygon = ego_polygon; if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + debug.failed_reason = "overlap_polygon"; return false; } @@ -2156,13 +2187,20 @@ bool isSafeInFreeSpaceCollisionCheck( util::getProjectedDistancePointFromPolygons( ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); + debug.expected_ego_pose = expected_ego_pose; + debug.expected_obj_pose = expected_obj_pose; + const auto object_twist = target_object.kinematics.initial_twist_with_covariance.twist; if (!util::hasEnoughDistance( expected_ego_pose, ego_current_twist, - target_object.kinematics.initial_pose_with_covariance.pose, object_twist, check_param)) { + target_object.kinematics.initial_pose_with_covariance.pose, object_twist, check_param, + debug)) { + debug.failed_reason = "not_enough_longitudinal"; return false; } } + + debug.failed_reason = ""; return true; } } // namespace util