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

feat(utils): add function to visualize drivable lanes #4282

Merged
merged 3 commits into from
Jul 20, 2023
Merged

feat(utils): add function to visualize drivable lanes #4282

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8a9de78
feat(utils): add function to visualize drivable lanes
satoshi-ota Jul 14, 2023
e836db8
Update planning/behavior_path_planner/src/marker_util/debug_utilities…
satoshi-ota Jul 20, 2023
fc1b0d0
Update planning/behavior_path_planner/src/marker_util/debug_utilities…
satoshi-ota Jul 20, 2023
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
5 changes: 5 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_
#define BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_

#include "behavior_path_planner/data_manager.hpp"
#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"

Expand All @@ -36,6 +37,7 @@ 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::DrivableLanes;
using behavior_path_planner::ShiftLineArray;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Polygon;
Expand Down Expand Up @@ -126,6 +128,9 @@ MarkerArray createObjectsMarkerArray(
const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r,
const float & g, const float & b);

MarkerArray createDrivableLanesMarkerArray(
const std::vector<DrivableLanes> & drivable_lanes, std::string && ns);

} // namespace marker_utils

#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTIL__DEBUG_UTILITIES_HPP_
45 changes: 45 additions & 0 deletions planning/behavior_path_planner/src/marker_util/debug_utilities.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -361,4 +361,49 @@ MarkerArray createObjectsMarkerArray(

return msg;
}

MarkerArray createDrivableLanesMarkerArray(
const std::vector<DrivableLanes> & drivable_lanes, std::string && ns)
{
MarkerArray msg;

int32_t i{0};

const auto get_lanelet_marker =
[&ns, &i](const auto & lanelet, const auto r, const auto g, const auto b) {
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, bitShift(lanelet.id()) + i++,
Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999));

if (!lanelet.polygon3d().empty()) {
marker.points.reserve(lanelet.polygon3d().size() + 1);
}

for (const auto & p : lanelet.polygon3d()) {
marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
}

if (!marker.points.empty()) {
marker.points.push_back(marker.points.front());
}

return marker;
};

for (const auto & drivable_lane : drivable_lanes) {
{
msg.markers.push_back(get_lanelet_marker(drivable_lane.right_lane, 1.0, 0.0, 0.0));
}

{
msg.markers.push_back(get_lanelet_marker(drivable_lane.left_lane, 0.0, 1.0, 0.0));
}

std::for_each(
drivable_lane.middle_lanes.begin(), drivable_lane.middle_lanes.end(),
[&](const auto & lane) { msg.markers.push_back(get_lanelet_marker(lane, 0.0, 0.0, 1.0)); });
}

return msg;
}
} // namespace marker_utils