Skip to content

Commit

Permalink
feat(avoidance): output road shoulder distance marker
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Dec 7, 2023
1 parent dd95c60 commit b779b30
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 50 deletions.
7 changes: 0 additions & 7 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,13 +59,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons

MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns);

MarkerArray makeOverhangToRoadShoulderMarkerArray(
const behavior_path_planner::ObjectDataArray & objects, std::string && ns);

MarkerArray createOverhangFurthestLineStringMarkerArray(
const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r,
const float & g, const float & b);

MarkerArray createDebugMarkerArray(
const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug);
} // namespace marker_utils::avoidance_marker
Expand Down
102 changes: 59 additions & 43 deletions planning/behavior_path_avoidance_module/src/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,42 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std:
return msg;
}

MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;

for (const auto & object : objects) {
if (!object.nearest_bound_point.has_value()) {
continue;
}

{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999));

marker.points.push_back(object.overhang_pose.position);
marker.points.push_back(object.nearest_bound_point.value());
marker.id = uuidToInt32(object.object.object_id);
msg.markers.push_back(marker);
}

{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));

marker.pose.position = object.nearest_bound_point.value();
std::ostringstream string_stream;
string_stream << object.to_road_shoulder_distance << "[m]";
marker.text = string_stream.str();
msg.markers.push_back(marker);
}
}

return msg;
}

MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns)
{
MarkerArray msg;
Expand Down Expand Up @@ -166,6 +202,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st

appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);

return msg;
}
Expand All @@ -183,6 +220,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::

appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);

return msg;
}
Expand Down Expand Up @@ -418,58 +456,37 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
return msg;
}

MarkerArray makeOverhangToRoadShoulderMarkerArray(
const ObjectDataArray & objects, std::string && ns)
{
auto marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 1.0));

int32_t i{0};
MarkerArray msg;
for (const auto & object : objects) {
marker.id = ++i;
marker.pose = object.overhang_pose;
std::ostringstream string_stream;
string_stream << "(to_road_shoulder_distance = " << object.to_road_shoulder_distance << " [m])";
marker.text = string_stream.str();
msg.markers.push_back(marker);
}

return msg;
}

MarkerArray createOverhangFurthestLineStringMarkerArray(
const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r,
const float & g, const float & b)
MarkerArray createDrivableBounds(
const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g,
const float & b)
{
const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
MarkerArray msg;

for (const auto & linestring : linestrings) {
const auto id = static_cast<int>(linestring.id());
// right bound
{
auto marker = createDefaultMarker(
"map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0),
"map", current_time, ns + "_right", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0),
createMarkerColor(r, g, b, 0.999));

marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
for (const auto & p : linestring.basicLineString()) {
for (const auto & p : data.right_bound) {
marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
}

msg.markers.push_back(marker);
}

// left bound
{
auto marker = createDefaultMarker(
"map", current_time, ns + "_left", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0),
createMarkerColor(r, g, b, 0.999));

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

Marker marker_linestring_id = createDefaultMarker(
"map", current_time, "linestring_id", id, Marker::TEXT_VIEW_FACING,
createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.8));
Pose text_id_pose;
text_id_pose.position.x = linestring.front().x();
text_id_pose.position.y = linestring.front().y();
text_id_pose.position.z = linestring.front().z();
marker_linestring_id.pose = text_id_pose;
std::ostringstream ss;
ss << "(ID : " << id << ") ";
marker_linestring_id.text = ss.str();
msg.markers.push_back(marker_linestring_id);
msg.markers.push_back(marker);
}

return msg;
Expand Down Expand Up @@ -593,8 +610,7 @@ MarkerArray createDebugMarkerArray(
add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));
add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang"));
add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0));
add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42));
}

return msg;
Expand Down

0 comments on commit b779b30

Please sign in to comment.