Skip to content

Commit

Permalink
refactor(behavior_path_planner): use debug_marker of interface for pu…
Browse files Browse the repository at this point in the history 
…ll_over (autowarefoundation#1843)

* refactor(behavior_path_planner): use debug_marker of interface for pull_over

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix typos

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55 @boyali
kosuke55 authored and boyali committed Oct 19, 2022
1 parent af2ba95 commit 5ff108c
Show file tree
Hide file tree
Showing 6 changed files with 105 additions and 117 deletions.
10 changes: 2 additions & 8 deletions ...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,12 +167,7 @@ class PullOverModule : public SceneModuleInterface
const double check_distance_ = 100.0;

rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_sub_;
rclcpp::Publisher<PoseStamped>::SharedPtr Cr_pub_;
rclcpp::Publisher<PoseStamped>::SharedPtr Cl_pub_;
rclcpp::Publisher<PoseStamped>::SharedPtr start_pose_pub_;
rclcpp::Publisher<PoseStamped>::SharedPtr goal_pose_pub_;
rclcpp::Publisher<PoseArray>::SharedPtr path_pose_array_pub_;
rclcpp::Publisher<MarkerArray>::SharedPtr parking_area_pub_;

PUllOverStatus status_;
OccupancyGridBasedCollisionDetector occupancy_grid_map_;
Expand Down Expand Up @@ -207,7 +202,7 @@ class PullOverModule : public SceneModuleInterface
void updateOccupancyGrid();
void researchGoal();
void resetStatus();
bool checkCollisionWtihLongitudinalDistance(
bool checkCollisionWithLongitudinalDistance(
const Pose & ego_pose, const PredictedObjects & dynamic_objects) const;
bool checkCollisionWithPose(const Pose & pose) const;
bool checkCollisionWithPath(const PathWithLaneId & path) const;
Expand All @@ -217,8 +212,7 @@ class PullOverModule : public SceneModuleInterface
std::pair<TurnIndicatorsCommand, double> 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
Expand Down
6 changes: 6 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
23 changes: 11 additions & 12 deletions ...h_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ class GeometricParallelParking
bool isParking() const;
bool planPullOver(
const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
const lanelet::ConstLanelets & shoulder_lanesconst, const bool is_forward);
const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward);
bool planPullOut(
const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
const lanelet::ConstLanelets & shoulder_lanes);
Expand All @@ -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<const PlannerData> planner_data_;
Expand All @@ -105,7 +104,7 @@ class GeometricParallelParking
bool isEnoughDistanceToStart(const Pose & start_pose) const;
std::vector<PathWithLaneId> planOneTrial(
const Pose & start_pose, const Pose & goal_pose, const double R_E_r,
const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanesconst,
const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
bool is_forward, const double end_pose_offset, const double lane_departure_margin);
PathWithLaneId generateArcPath(
const Pose & center, const double radius, const double start_yaw, double end_yaw,
Expand All @@ -123,11 +122,11 @@ class GeometricParallelParking
PathWithLaneId generateStraightPath(const Pose & start_pose);
void setVelocityToArcPaths(std::vector<PathWithLaneId> & 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

Expand Down
137 changes: 51 additions & 86 deletions planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,12 +54,6 @@ PullOverModule::PullOverModule(
rtc_interface_ptr_ = std::make_shared<RTCInterface>(&node, "pull_over");
goal_pose_pub_ =
node.create_publisher<PoseStamped>("/planning/scenario_planning/modified_goal", 1);
parking_area_pub_ = node.create_publisher<MarkerArray>("~/pull_over/debug/parking_area", 1);
// Only for arc paths
Cl_pub_ = node.create_publisher<PoseStamped>("~/pull_over/debug/Cl", 1);
Cr_pub_ = node.create_publisher<PoseStamped>("~/pull_over/debug/Cr", 1);
start_pose_pub_ = node.create_publisher<PoseStamped>("~/pull_over/debug/start_pose", 1);
path_pose_array_pub_ = node.create_publisher<PoseArray>("~/pull_over/debug/path_pose_array", 1);

lane_departure_checker_ = std::make_unique<LaneDepartureChecker>();
lane_departure_checker_->setVehicleInfo(
Expand Down Expand Up @@ -266,7 +260,7 @@ void PullOverModule::researchGoal()

const auto objects_in_shoulder_lane =
util::filterObjectsByLanelets(*(planner_data_->dynamic_object), status_.pull_over_lanes);
if (checkCollisionWtihLongitudinalDistance(search_pose, objects_in_shoulder_lane)) {
if (checkCollisionWithLongitudinalDistance(search_pose, objects_in_shoulder_lane)) {
continue;
}

Expand Down Expand Up @@ -317,15 +311,15 @@ bool PullOverModule::isLongEnoughToParkingStart(
return *dist_to_parking_start_pose > current_to_stop_distance;
}

bool PullOverModule::checkCollisionWtihLongitudinalDistance(
bool PullOverModule::checkCollisionWithLongitudinalDistance(
const Pose & ego_pose, const PredictedObjects & dynamic_objects) const
{
if (parameters_.use_occupancy_grid) {
bool check_out_of_range = false;
const double offset = std::max(
parameters_.goal_to_obstacle_margin - parameters_.occupancy_grid_collision_check_margin, 0.0);

// check forward collison
// check forward collision
const Pose ego_pose_moved_forward = calcOffsetPose(ego_pose, offset, 0, 0);
const Pose forward_pose_grid_coords =
global2local(occupancy_grid_map_.getMap(), ego_pose_moved_forward);
Expand All @@ -336,7 +330,7 @@ bool PullOverModule::checkCollisionWtihLongitudinalDistance(
return true;
}

// check backward collison
// check backward collision
const Pose ego_pose_moved_backward = calcOffsetPose(ego_pose, -offset, 0, 0);
const Pose backward_pose_grid_coords =
global2local(occupancy_grid_map_.getMap(), ego_pose_moved_backward);
Expand Down Expand Up @@ -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())) {
Expand All @@ -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())) {
Expand Down Expand Up @@ -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())) {
Expand All @@ -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())) {
Expand All @@ -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;
}
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -982,7 +980,7 @@ std::pair<TurnIndicatorsCommand, double> 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();
}
}

Expand All @@ -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) {
Expand All @@ -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
Expand Down
31 changes: 31 additions & 0 deletions planning/behavior_path_planner/src/scene_module/pull_over/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Loading

0 comments on commit 5ff108c

Please sign in to comment.