Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(behavior_path_planner): use debug_marker of interface for pull_over #1843

Merged
merged 2 commits into from
Sep 12, 2022
Merged

refactor(behavior_path_planner): use debug_marker of interface for pull_over #1843

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
44c78d0
refactor(behavior_path_planner): use debug_marker of interface for pu…
kosuke55 Sep 12, 2022
452cc26
fix typos
kosuke55 Sep 12, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 2 additions & 8 deletions ...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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