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.

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 add polyline label id info #27

Open
wants to merge 2 commits into
base: main
Choose a base branch
from

Conversation

danielsanchezaran
Copy link
Collaborator

This PR adds label ids to the polilyne construction so it is not 0.0 by default

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Copy link
Owner

@ktro2828 ktro2828 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@danielsanchezaran Thank you for your PR. I left some minor suggestions. Please check them out.

@@ -135,7 +217,8 @@ std::vector<LanePoint> LaneletConverter::fromPolygon(
dy /= (norm + epsilon);
dz /= (norm + epsilon);
}
output.emplace_back(itr->x(), itr->y(), itr->z(), dx, dy, dz, 0.0); // TODO(ktro2828): Label ID
output.emplace_back(
itr->x(), itr->y(), itr->z(), dx, dy, dz, label_id); // TODO(ktro2828): Label ID
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[NITS] TODO can be removed.

Suggested change
itr->x(), itr->y(), itr->z(), dx, dy, dz, label_id); // TODO(ktro2828): Label ID
itr->x(), itr->y(), itr->z(), dx, dy, dz, label_id);

Comment on lines +28 to +100

bool isVirtualLinestring(const std::string & line_type, const std::string & line_subtype)
{
// Indicate whether input linestring type and subtype is virtual.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line type is `virtual` and subtype is `""`.
return line_type == "virtual" && line_subtype == "";
}

bool isRoadedgeLinestring(const std::string & line_type)
{
// Indicate whether input linestring type and subtype is supported RoadEdge.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line type is contained in T4RoadEdge.
return std::find(T4_ROADEDGE.begin(), T4_ROADEDGE.end(), line_type) != T4_ROADEDGE.end();
}

bool isRoadlineLinestring(const std::string & line_subtype)
{
// Indicate whether input linestring type and subtype is supported RoadLine.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line subtype is contained in T4RoadLine.
// Note: Currently `line_type` is not used, but it might be used in the future.
return std::find(T4_ROADLINE.begin(), T4_ROADLINE.end(), line_subtype) != T4_ROADLINE.end();
}

std::string getLinestringType(const lanelet::ConstLineString3d & linestring)
{
if (linestring.hasAttribute("type")) {
return linestring.attribute("type").value();
} else {
return "";
}
}

std::string getLinestringSubtype(const lanelet::ConstLineString3d & linestring)
{
if (linestring.hasAttribute("subtype")) {
return linestring.attribute("subtype").value();
} else {
return "";
}
}

float getBoundaryType(const lanelet::ConstLineString3d & linestring)
{
// Return the `BoundaryType` from linestring.
// Args:
// linestring (lanelet::ConstLineString3d): LineString instance.
// Returns:
// MapType: Boundary type.
auto line_type = getLinestringType(linestring);
auto line_subtype = getLinestringSubtype(linestring);
if (isVirtualLinestring(line_type, line_subtype)) {
return static_cast<float>(MapType::UNKNOWN);
} else if (isRoadedgeLinestring(line_type)) {
return static_cast<float>(autoware::mtr::g_map_type_mapping.at(line_type));
} else if (isRoadlineLinestring(line_subtype)) {
return static_cast<float>(autoware::mtr::g_map_type_mapping.at(line_subtype));
} else {
return static_cast<float>(MapType::UNKNOWN);
}
}

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[NITS] It would be better to separate functions only used in the source file using anonymous namespace.

Suggested change
bool isVirtualLinestring(const std::string & line_type, const std::string & line_subtype)
{
// Indicate whether input linestring type and subtype is virtual.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line type is `virtual` and subtype is `""`.
return line_type == "virtual" && line_subtype == "";
}
bool isRoadedgeLinestring(const std::string & line_type)
{
// Indicate whether input linestring type and subtype is supported RoadEdge.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line type is contained in T4RoadEdge.
return std::find(T4_ROADEDGE.begin(), T4_ROADEDGE.end(), line_type) != T4_ROADEDGE.end();
}
bool isRoadlineLinestring(const std::string & line_subtype)
{
// Indicate whether input linestring type and subtype is supported RoadLine.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line subtype is contained in T4RoadLine.
// Note: Currently `line_type` is not used, but it might be used in the future.
return std::find(T4_ROADLINE.begin(), T4_ROADLINE.end(), line_subtype) != T4_ROADLINE.end();
}
std::string getLinestringType(const lanelet::ConstLineString3d & linestring)
{
if (linestring.hasAttribute("type")) {
return linestring.attribute("type").value();
} else {
return "";
}
}
std::string getLinestringSubtype(const lanelet::ConstLineString3d & linestring)
{
if (linestring.hasAttribute("subtype")) {
return linestring.attribute("subtype").value();
} else {
return "";
}
}
float getBoundaryType(const lanelet::ConstLineString3d & linestring)
{
// Return the `BoundaryType` from linestring.
// Args:
// linestring (lanelet::ConstLineString3d): LineString instance.
// Returns:
// MapType: Boundary type.
auto line_type = getLinestringType(linestring);
auto line_subtype = getLinestringSubtype(linestring);
if (isVirtualLinestring(line_type, line_subtype)) {
return static_cast<float>(MapType::UNKNOWN);
} else if (isRoadedgeLinestring(line_type)) {
return static_cast<float>(autoware::mtr::g_map_type_mapping.at(line_type));
} else if (isRoadlineLinestring(line_subtype)) {
return static_cast<float>(autoware::mtr::g_map_type_mapping.at(line_subtype));
} else {
return static_cast<float>(MapType::UNKNOWN);
}
}
namespace
{
bool isVirtualLinestring(const std::string & line_type, const std::string & line_subtype)
{
// Indicate whether input linestring type and subtype is virtual.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line type is `virtual` and subtype is `""`.
return line_type == "virtual" && line_subtype == "";
}
bool isRoadedgeLinestring(const std::string & line_type)
{
// Indicate whether input linestring type and subtype is supported RoadEdge.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line type is contained in T4RoadEdge.
return std::find(T4_ROADEDGE.begin(), T4_ROADEDGE.end(), line_type) != T4_ROADEDGE.end();
}
bool isRoadlineLinestring(const std::string & line_subtype)
{
// Indicate whether input linestring type and subtype is supported RoadLine.
// Args:
// line_type (str): Line type name.
// line_subtype (str): Line subtype name.
// Returns:
// bool: Return True if line subtype is contained in T4RoadLine.
// Note: Currently `line_type` is not used, but it might be used in the future.
return std::find(T4_ROADLINE.begin(), T4_ROADLINE.end(), line_subtype) != T4_ROADLINE.end();
}
std::string getLinestringType(const lanelet::ConstLineString3d & linestring)
{
if (linestring.hasAttribute("type")) {
return linestring.attribute("type").value();
} else {
return "";
}
}
std::string getLinestringSubtype(const lanelet::ConstLineString3d & linestring)
{
if (linestring.hasAttribute("subtype")) {
return linestring.attribute("subtype").value();
} else {
return "";
}
}
float getBoundaryType(const lanelet::ConstLineString3d & linestring)
{
// Return the `BoundaryType` from linestring.
// Args:
// linestring (lanelet::ConstLineString3d): LineString instance.
// Returns:
// MapType: Boundary type.
auto line_type = getLinestringType(linestring);
auto line_subtype = getLinestringSubtype(linestring);
if (isVirtualLinestring(line_type, line_subtype)) {
return static_cast<float>(MapType::UNKNOWN);
} else if (isRoadedgeLinestring(line_type)) {
return static_cast<float>(autoware::mtr::g_map_type_mapping.at(line_type));
} else if (isRoadlineLinestring(line_subtype)) {
return static_cast<float>(autoware::mtr::g_map_type_mapping.at(line_subtype));
} else {
return static_cast<float>(MapType::UNKNOWN);
}
}
} // namespace

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants