From 44c78d004094356c8912286d49ddf79e3ab9bf09 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 12 Sep 2022 22:05:58 +0900 Subject: [PATCH] refactor(behavior_path_planner): use debug_marker of interface for pull_over Signed-off-by: kosuke55 --- .../pull_over/pull_over_module.hpp | 8 +- .../scene_module/pull_over/util.hpp | 6 + .../utils/geometric_parallel_parking.hpp | 19 ++- .../pull_over/pull_over_module.cpp | 129 +++++++----------- .../src/scene_module/pull_over/util.cpp | 31 +++++ .../utils/geometric_parallel_parking.cpp | 15 +- 6 files changed, 98 insertions(+), 110 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 9616777a4d015..498d9429f130a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -167,12 +167,7 @@ class PullOverModule : public SceneModuleInterface const double check_distance_ = 100.0; rclcpp::Subscription::SharedPtr occupancy_grid_sub_; - rclcpp::Publisher::SharedPtr Cr_pub_; - rclcpp::Publisher::SharedPtr Cl_pub_; - rclcpp::Publisher::SharedPtr start_pose_pub_; rclcpp::Publisher::SharedPtr goal_pose_pub_; - rclcpp::Publisher::SharedPtr path_pose_array_pub_; - rclcpp::Publisher::SharedPtr parking_area_pub_; PUllOverStatus status_; OccupancyGridBasedCollisionDetector occupancy_grid_map_; @@ -217,8 +212,7 @@ class PullOverModule : public SceneModuleInterface std::pair getTurnInfo() const; // debug - Marker createParkingAreaMarker(const Pose & back_pose, const Pose & front_pose, const int32_t id); - void publishDebugData(); + void setDebugData(); void printParkingPositionError() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp index 43ac1f9dc9536..7c8ba496bb55c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp @@ -62,6 +62,12 @@ bool hasEnoughDistance( const ShiftParkingPath & path, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose); lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler); + +// debug +Marker createPullOverAreaMarker( + const Pose & start_pose, const Pose & end_pose, const int32_t id, + const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, + const double vehicle_width, const std_msgs::msg::ColorRGBA & color); } // namespace pull_over_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp index a8f60036d7554..06ecdfc9d3421 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp @@ -84,11 +84,10 @@ class GeometricParallelParking PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; PathWithLaneId getArcPath() const; - PoseStamped getCr() const { return Cr_; } - PoseStamped getCl() const { return Cl_; } - PoseStamped getStartPose() const { return start_pose_; } - PoseStamped getArcEndPose() const { return arc_end_pose_; } - PoseArray getPathPoseArray() const { return path_pose_array_; } + Pose getCr() const { return Cr_; } + Pose getCl() const { return Cl_; } + Pose getStartPose() const { return start_pose_; } + Pose getArcEndPose() const { return arc_end_pose_; } private: std::shared_ptr planner_data_; @@ -123,11 +122,11 @@ class GeometricParallelParking PathWithLaneId generateStraightPath(const Pose & start_pose); void setVelocityToArcPaths(std::vector & arc_paths, const double velocity); - PoseStamped Cr_; - PoseStamped Cl_; - PoseStamped start_pose_; - PoseStamped arc_end_pose_; - PoseArray path_pose_array_; + // debug + Pose Cr_; + Pose Cl_; + Pose start_pose_; + Pose arc_end_pose_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index a2181c595761a..39a1e4f89d52f 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -54,12 +54,6 @@ PullOverModule::PullOverModule( rtc_interface_ptr_ = std::make_shared(&node, "pull_over"); goal_pose_pub_ = node.create_publisher("/planning/scenario_planning/modified_goal", 1); - parking_area_pub_ = node.create_publisher("~/pull_over/debug/parking_area", 1); - // Only for arc paths - Cl_pub_ = node.create_publisher("~/pull_over/debug/Cl", 1); - Cr_pub_ = node.create_publisher("~/pull_over/debug/Cr", 1); - start_pose_pub_ = node.create_publisher("~/pull_over/debug/start_pose", 1); - path_pose_array_pub_ = node.create_publisher("~/pull_over/debug/path_pose_array", 1); lane_departure_checker_ = std::make_unique(); lane_departure_checker_->setVehicleInfo( @@ -428,8 +422,7 @@ bool PullOverModule::planWithEfficientPath() parallel_parking_planner_.planPullOver( goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, true) && isLongEnoughToParkingStart( - parallel_parking_planner_.getCurrentPath(), - parallel_parking_planner_.getStartPose().pose) && + parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose()) && !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { @@ -450,8 +443,7 @@ bool PullOverModule::planWithEfficientPath() parallel_parking_planner_.planPullOver( goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, false) && isLongEnoughToParkingStart( - parallel_parking_planner_.getCurrentPath(), - parallel_parking_planner_.getStartPose().pose) && + parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose()) && !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { @@ -492,8 +484,7 @@ bool PullOverModule::planWithCloseGoal() parallel_parking_planner_.planPullOver( goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, true) && isLongEnoughToParkingStart( - parallel_parking_planner_.getCurrentPath(), - parallel_parking_planner_.getStartPose().pose) && + parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose()) && !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { @@ -510,8 +501,7 @@ bool PullOverModule::planWithCloseGoal() parallel_parking_planner_.planPullOver( goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, false) && isLongEnoughToParkingStart( - parallel_parking_planner_.getCurrentPath(), - parallel_parking_planner_.getStartPose().pose) && + parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose()) && !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { @@ -538,7 +528,7 @@ Pose PullOverModule::getParkingStartPose() const shift_parking_path_.path.points, shift_parking_path_.shift_point.start.position, -offset); parking_start_pose = offset_pose ? *offset_pose : shift_parking_path_.shift_point.start; } else if (isArcPath()) { - parking_start_pose = parallel_parking_planner_.getStartPose().pose; + parking_start_pose = parallel_parking_planner_.getStartPose(); } return parking_start_pose; } @@ -673,7 +663,15 @@ BehaviorModuleOutput PullOverModule::plan() const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); - publishDebugData(); + setDebugData(); + + // Publish the modified goal only when its path is safe. + if (status_.is_safe) { + PoseStamped goal_pose_stamped; + goal_pose_stamped.header = planner_data_->route_handler->getRouteHeader(); + goal_pose_stamped.pose = modified_goal_pose_; + goal_pose_pub_->publish(goal_pose_stamped); + } // For evaluations if (parameters_.print_debug_info) { @@ -982,7 +980,7 @@ std::pair PullOverModule::getTurnInfo() const if (status_.path_type == PathType::SHIFT) { parking_end_pose = shift_parking_path_.shift_point.end; } else if (isArcPath()) { - parking_end_pose = parallel_parking_planner_.getArcEndPose().pose; + parking_end_pose = parallel_parking_planner_.getArcEndPose(); } } @@ -1007,51 +1005,17 @@ bool PullOverModule::isArcPath() const return status_.path_type == PathType::ARC_FORWARD || status_.path_type == PathType::ARC_BACKWARD; } -Marker PullOverModule::createParkingAreaMarker( - const Pose & start_pose, const Pose & end_pose, const int32_t id) +void PullOverModule::setDebugData() { - const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - Marker marker = createDefaultMarker( - "map", planner_data_->route_handler->getRouteHeader().stamp, "collision_polygons", id, - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); - - auto p_left_front = calcOffsetPose( - end_pose, planner_data_->parameters.base_link2front, - planner_data_->parameters.vehicle_width / 2, 0) - .position; - marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); - - auto p_right_front = calcOffsetPose( - end_pose, planner_data_->parameters.base_link2front, - -planner_data_->parameters.vehicle_width / 2, 0) - .position; - marker.points.push_back(createPoint(p_right_front.x, p_right_front.y, p_right_front.z)); - - auto p_right_back = calcOffsetPose( - start_pose, -planner_data_->parameters.base_link2rear, - -planner_data_->parameters.vehicle_width / 2, 0) - .position; - marker.points.push_back(createPoint(p_right_back.x, p_right_back.y, p_right_back.z)); - - auto p_left_back = calcOffsetPose( - start_pose, -planner_data_->parameters.base_link2rear, - planner_data_->parameters.vehicle_width / 2, 0) - .position; - marker.points.push_back(createPoint(p_left_back.x, p_left_back.y, p_left_back.z)); - marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); - - return marker; -} + debug_marker_.markers.clear(); -void PullOverModule::publishDebugData() -{ - auto header = planner_data_->route_handler->getRouteHeader(); - // Publish the modified goal only when it's path is safe. - PoseStamped goal_pose_stamped; - goal_pose_stamped.header = header; - if (status_.is_safe) goal_pose_stamped.pose = modified_goal_pose_; - goal_pose_pub_->publish(goal_pose_stamped); + using marker_utils::createPathMarkerArray; + using marker_utils::createPolygonMarkerArray; + using marker_utils::createPoseMarkerArray; + + const auto add = [this](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + }; // Visualize pull over areas if (parameters_.enable_goal_research) { @@ -1060,31 +1024,32 @@ void PullOverModule::publishDebugData() calcOffsetPose(refined_goal_pose, -parameters_.backward_goal_search_length, 0, 0); const Pose end_pose = calcOffsetPose(refined_goal_pose, parameters_.forward_goal_search_length, 0, 0); - MarkerArray marker_array; - marker_array.markers.push_back(createParkingAreaMarker(start_pose, end_pose, 0)); - - parking_area_pub_->publish(marker_array); + // marker_array.markers.push_back(createParkingAreaMarker(start_pose, end_pose, 0)); + const auto header = planner_data_->route_handler->getRouteHeader(); + const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const auto p = planner_data_->parameters; + debug_marker_.markers.push_back(pull_over_utils::createPullOverAreaMarker( + start_pose, end_pose, 0, header, p.base_link2front, p.base_link2rear, p.vehicle_width, + color)); } - // Only for arc paths. Initialize data not to publish them when using other path. - PoseStamped Cl, Cr, start_pose; - PoseArray path_pose_array; - Cl.header = header; - Cr.header = header; - start_pose.header = header; - path_pose_array.header = header; - if (isArcPath() && status_.is_safe) { - Cl = parallel_parking_planner_.getCl(); - Cr = parallel_parking_planner_.getCr(); - start_pose = parallel_parking_planner_.getStartPose(); - path_pose_array = parallel_parking_planner_.getPathPoseArray(); - } else if (status_.is_safe) { - path_pose_array = util::convertToGeometryPoseArray(status_.path); + // Visualize path and related pose + if (!status_.is_safe) { + return; + } + if (isArcPath()) { + add(createPoseMarkerArray(parallel_parking_planner_.getCl(), "Cl", 0, 0.9, 0.3, 0.3)); + add(createPoseMarkerArray(parallel_parking_planner_.getCr(), "Cr", 0, 0.3, 0.9, 0.3)); + add(createPoseMarkerArray( + parallel_parking_planner_.getStartPose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + add(createPoseMarkerArray( + parallel_parking_planner_.getArcEndPose(), "pull_over_arc_end_pose", 0, 0.9, 0.9, 0.3)); + add(createPathMarkerArray( + parallel_parking_planner_.getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + } else { + add(createPathMarkerArray(status_.path, "full_path", 0, 0.0, 0.5, 0.9)); } - Cl_pub_->publish(Cl); - Cr_pub_->publish(Cr); - start_pose_pub_->publish(start_pose); - path_pose_array_pub_->publish(path_pose_array); } void PullOverModule::printParkingPositionError() const diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index 067cc03f91242..bbe78c0d3459c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -343,5 +343,36 @@ lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler) } return pull_over_lanes; } + +Marker createPullOverAreaMarker( + const Pose & start_pose, const Pose & end_pose, const int32_t id, + const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, + const double vehicle_width, const std_msgs::msg::ColorRGBA & color) +{ + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerScale; + using tier4_autoware_utils::createPoint; + + Marker marker = createDefaultMarker( + header.frame_id, header.stamp, "pull_over_area", id, + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); + + auto p_left_front = calcOffsetPose(end_pose, base_link2front, vehicle_width / 2, 0).position; + marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); + + auto p_right_front = calcOffsetPose(end_pose, base_link2front, -vehicle_width / 2, 0).position; + marker.points.push_back(createPoint(p_right_front.x, p_right_front.y, p_right_front.z)); + + auto p_right_back = calcOffsetPose(start_pose, -base_link2rear, -vehicle_width / 2, 0).position; + marker.points.push_back(createPoint(p_right_back.x, p_right_back.y, p_right_back.z)); + + auto p_left_back = calcOffsetPose(start_pose, -base_link2rear, vehicle_width / 2, 0).position; + marker.points.push_back(createPoint(p_left_back.x, p_left_back.y, p_left_back.z)); + + marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); + + return marker; +} + } // namespace pull_over_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 4b726c66f4489..d50f79d67035b 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -174,7 +174,6 @@ void GeometricParallelParking::clearPaths() { current_path_idx_ = 0; arc_paths_.clear(); - path_pose_array_.poses.clear(); paths_.clear(); } @@ -448,16 +447,10 @@ std::vector GeometricParallelParking::planOneTrial( paths_.push_back(path_turn_right); // debug - Cr_.pose = Cr; - Cr_.header = planner_data_->route_handler->getRouteHeader(); - Cl_.pose = Cl; - Cl_.header = planner_data_->route_handler->getRouteHeader(); - start_pose_.pose = start_pose; - start_pose_.header = planner_data_->route_handler->getRouteHeader(); - arc_end_pose_.pose = arc_end_pose; - arc_end_pose_.header = planner_data_->route_handler->getRouteHeader(); - path_pose_array_ = convertToGeometryPoseArray(getFullPath()); - path_pose_array_.header = planner_data_->route_handler->getRouteHeader(); + Cr_ = Cr; + Cl_ = Cl; + start_pose_ = start_pose; + arc_end_pose_ = arc_end_pose; return paths_; }