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鈥檒l occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adds a adjacency checker for the builder. #45

Merged
merged 5 commits into from Dec 23, 2022
Merged

Adds a adjacency checker for the builder. #45

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
791e285
Adds a adjacency checker for the builder.
francocipollone Dec 12, 2022
ad67ef8
Addresses comments.
francocipollone Dec 19, 2022
68c48cf
Addresses comments
francocipollone Dec 21, 2022
be8e284
Addresses comments.
francocipollone Dec 22, 2022
eeb7c16
Addresses final comments.
francocipollone Dec 23, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
35 changes: 16 additions & 19 deletions include/maliput_sparse/parser/validator.h
View file
Open in desktop
Expand Up @@ -60,7 +60,8 @@ class Validator {
struct Error {
/// The type of error.
enum class Type {
kLaneAdjacency,
kLogicalLaneAdjacency,
kGeometricalLaneAdjacency,
};
/// The severity of the error.
enum class Severity {
Expand All @@ -87,29 +88,25 @@ class Validator {
/// @param parser The maliput_sparse::parser::Parser instance to validate.
/// @param options The maliput_sparse::parser::ValidatorOptions to use.
/// @param config The maliput_sparse::parser::ValidatorConfig to use.
Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig config);
Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig& config);

/// Returns the errors found during validation.
const std::vector<Error>& GetErrors() const;
std::vector<Error> operator()() const;

private:
// Helper functions for reporting errors.
// @param message The error message.
// @Param type The type of error.
// @Param severity The severity of the error.
void Report(const std::string& message, const Error::Type& type, const Error::Severity& severity);

// Method to validate lane adjacency.
// @param parser The maliput_sparse::parser::Parser instance to validate.
// @param config The maliput_sparse::parser::ValidatorConfig to use.
void ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config);

// Helper function to geometrically check lane adjacency.
void CheckAdjacency(const Lane& lane, const Lane& adjacent_lane, bool left, double tolerance);

// Holds the errors found during validation.
std::vector<Error> errors_;
// The maliput_sparse::parser::Parser instance to validate.
const Parser* parser_{nullptr};
// The maliput_sparse::parser::ValidatorOptions to use.
const ValidatorOptions options_;
// The maliput_sparse::parser::ValidatorConfig to use.
const ValidatorConfig config_;
};

/// Validates lane adjacency.
/// @param parser The maliput_sparse::parser::Parser instance to validate.
/// @param config The maliput_sparse::parser::ValidatorConfig to use.
/// @returns A vector of maliput_sparse::parser::Validator::Error.
std::vector<Validator::Error> ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config);

agalbachicar marked this conversation as resolved.
Show resolved Hide resolved
} // namespace parser
} // namespace maliput_sparse
13 changes: 7 additions & 6 deletions src/geometry/utility/geometry.cc
View file
Open in desktop
Expand Up @@ -60,6 +60,7 @@

#include <algorithm>
#include <cmath>
#include <numeric>

namespace maliput_sparse {
namespace geometry {
Expand Down Expand Up @@ -377,12 +378,12 @@ ClosestPointResult GetClosestPoint(const LineString3d& line_string, const malipu
double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs) {
const LineString3d& base_ls = rhs.size() > lhs.size() ? rhs : lhs;
const LineString3d& other_ls = rhs.size() > lhs.size() ? lhs : rhs;
double sum_distances{};
for (const auto& base_point : base_ls) {
const auto closest_point_res = GetClosestPoint(other_ls, base_point);
sum_distances += closest_point_res.distance;
}
return sum_distances / base_ls.size();
const double sum_distances =
std::accumulate(base_ls.begin(), base_ls.end(), 0.0, [&other_ls](double sum, const auto& base_point) {
const auto closest_point_res = GetClosestPoint(other_ls, base_point);
return sum + closest_point_res.distance;
});
return sum_distances / static_cast<double>(base_ls.size());
}

} // namespace utility
Expand Down
19 changes: 11 additions & 8 deletions src/loader/road_geometry_loader.cc
View file
Open in desktop
Expand Up @@ -59,15 +59,18 @@ RoadGeometryLoader::RoadGeometryLoader(std::unique_ptr<parser::Parser> parser,

std::unique_ptr<const maliput::api::RoadGeometry> RoadGeometryLoader::operator()() {
// Validates the parsed data before building the RoadGeometry.
const parser::Validator validator(parser_.get(), parser::ValidatorOptions{true},
parser::ValidatorConfig{builder_configuration_.linear_tolerance});
const auto errors = validator.GetErrors();
const auto errors = parser::Validator(parser_.get(), parser::ValidatorOptions{true},
parser::ValidatorConfig{builder_configuration_.linear_tolerance})();
for (const auto& error : errors) {
if (error.severity == parser::Validator::Error::Severity::kError) {
maliput::log()->error("{}", error.message);
}
if (error.severity == parser::Validator::Error::Severity::kWarning) {
maliput::log()->warn("{}", error.message);
switch (error.severity) {
case parser::Validator::Error::Severity::kError:
maliput::log()->error("{}", error.message);
break;
case parser::Validator::Error::Severity::kWarning:
maliput::log()->warn("{}", error.message);
break;
default:
MALIPUT_THROW_MESSAGE("Unknown parser::Validator::Error::Severity value: " + static_cast<int>(error.severity));
}
}

Expand Down
184 changes: 91 additions & 93 deletions src/parser/validator.cc
View file
Open in desktop
Expand Up @@ -43,35 +43,41 @@ namespace {
static constexpr bool kLeft{true};
static constexpr bool kRight{!kLeft};

} // namespace

Validator::Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig config) {
MALIPUT_THROW_UNLESS(parser);
if (options.lane_adjacency) {
ValidateLaneAdjacency(parser, config);
}
}
// Helper function to geometrically check lane adjacency.
// It relies on geometry::utility::ComputeDistance to compute the distance between the two adjacent line strings.
// Returns true if the lanes are adjacent under certain tolerance, false otherwise.

const std::vector<Validator::Error>& Validator::GetErrors() const { return errors_; }

void Validator::CheckAdjacency(const Lane& lane, const Lane& adjacent_lane, bool left, double tolerance) {
bool AreAdjacent(const Lane& lane, const Lane& adjacent_lane, bool left, double tolerance) {
const auto line_string_a = left ? lane.left : lane.right;
const auto line_string_b = left ? adjacent_lane.right : adjacent_lane.left;
const double distance = geometry::utility::ComputeDistance(line_string_a, line_string_b);
if (distance > tolerance) {
const std::string msg{"Lane " + lane.id + " and lane " + adjacent_lane.id +
" are not adjacent under the tolerance " + std::to_string(tolerance) +
". (distance: " + std::to_string(distance) + ")"};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
}
return distance <= tolerance;
}

void Validator::Report(const std::string& message, const Validator::Error::Type& type,
const Validator::Error::Severity& severity) {
errors_.push_back(Validator::Error{message, type, severity});
} // namespace

Validator::Validator(const Parser* parser, const ValidatorOptions& options, const ValidatorConfig& config)
: parser_{parser}, options_{options}, config_{config} {
MALIPUT_THROW_UNLESS(parser_);
}

std::vector<Validator::Error> Validator::operator()() const {
std::vector<Validator::Error> errors;
if (options_.lane_adjacency) {
const auto lane_adjacency_errors = ValidateLaneAdjacency(parser_, config_);
errors.insert(errors.end(), lane_adjacency_errors.begin(), lane_adjacency_errors.end());
}
return errors;
}

void Validator::ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config) {
std::vector<Validator::Error> ValidateLaneAdjacency(const Parser* parser, const ValidatorConfig config) {
std::vector<Validator::Error> errors;
auto evaluate = [&errors](bool condition, const std::string& message, const Validator::Error::Type& error_type) {
if (condition) {
errors.push_back({message, error_type, Validator::Error::Severity::kError});
}
return condition;
};
const auto junctions = parser->GetJunctions();
for (const auto junction : junctions) {
for (const auto segment : junction.second.segments) {
Expand All @@ -82,130 +88,122 @@ void Validator::ValidateLaneAdjacency(const Parser* parser, const ValidatorConfi
const Lane& lane = lanes[idx];

// Note: The following code is a bit repetitive for left and right adjacency checks, but it is easier to read
// and understand. Check right adjacency <--------------------------------------------------
// and understand.
//
// Check right adjacency <--------------------------------------------------
if (lane.right_lane_id) {
// Check if there is a idx to the right.
if (idx == 0) {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" +
lane.right_lane_id.value() + ") but is the first lane in the segment " +
segment.first + "."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
}
evaluate(idx == 0,
{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() +
") but is the first lane in the segment " + segment.first + "."},
Validator::Error::Type::kLogicalLaneAdjacency);
// Check if right lane id is in the same segment
const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) {
return lane.right_lane_id.value() == lane_it.id;
});
if (adj_lane_it == lanes.end()) {
// Error.
const std::string msg{"Adjacent lane isn't part of the segment: Lane " + lane.id +
" has a right lane id (" + lane.right_lane_id.value() +
") that is not part of the segment " + segment.first + "."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
evaluate(true,
{"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a right lane id (" +
lane.right_lane_id.value() + ") that is not part of the segment " + segment.first + "."},
Validator::Error::Type::kLogicalLaneAdjacency);
} else {
agalbachicar marked this conversation as resolved.
Show resolved Hide resolved
// Check if right lane id has the lane id as left lane id.
if (adj_lane_it->left_lane_id) {
agalbachicar marked this conversation as resolved.
Show resolved Hide resolved
if (adj_lane_it->left_lane_id.value() != lane.id) {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" +
lane.right_lane_id.value() + ") that has a left lane id (" +
adj_lane_it->left_lane_id.value() + ") that is not the lane " + lane.id + "."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
}
evaluate(adj_lane_it->left_lane_id.value() != lane.id,
{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" +
lane.right_lane_id.value() + ") that has a left lane id (" + adj_lane_it->left_lane_id.value() +
") that is not the lane " + lane.id + "."},
Validator::Error::Type::kLogicalLaneAdjacency);

} else {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" +
lane.right_lane_id.value() + ") that has no left lane id."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
evaluate(true,
{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" +
lane.right_lane_id.value() + ") that has no left lane id."},
Validator::Error::Type::kLogicalLaneAdjacency);
}

// Check geometrical adjacency.
CheckAdjacency(lane, *adj_lane_it, kRight, config.linear_tolerance);
evaluate(!AreAdjacent(lane, *adj_lane_it, kRight, config.linear_tolerance),
{"Lane " + lane.id + " and lane " + adj_lane_it->id + " are not adjacent under the tolerance " +
std::to_string(config.linear_tolerance) + "."},
Validator::Error::Type::kGeometricalLaneAdjacency);
}

// Check if idx - 1 lane is the right lane id.
if ((idx - 1 >= 0) && (lanes[idx - 1].id != lane.right_lane_id.value())) {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" +
lane.right_lane_id.value() + ") that is not the previous lane in the segment " +
segment.first + "."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
}
evaluate((idx - 1 >= 0) && (lanes[idx - 1].id != lane.right_lane_id.value()),
{"Wrong ordering of lanes: Lane " + lane.id + " has a right lane id (" + lane.right_lane_id.value() +
") that is not the previous lane in the segment " + segment.first + "."},
Validator::Error::Type::kLogicalLaneAdjacency);

} else {
// Check if idx is the first lane in the segment.
if (idx != 0) {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id +
" has no right lane id but it isn't the first lane in the segment " + segment.first +
"."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
}
evaluate(idx != 0,
{"Wrong ordering of lanes: Lane " + lane.id +
" has no right lane id but it isn't the first lane in the segment " + segment.first + "."},
Validator::Error::Type::kLogicalLaneAdjacency);
}

// Check left adjacency <--------------------------------------------------
if (lane.left_lane_id) {
// Check if there is a idx to the left.
if (idx == static_cast<int>(lanes.size()) - 1) {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" +
lane.left_lane_id.value() + ") but is the last lane in the segment " + segment.first +
"."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
}
evaluate(idx == static_cast<int>(lanes.size()) - 1,
{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() +
") but is the last lane in the segment " + segment.first + "."},
Validator::Error::Type::kLogicalLaneAdjacency);

// Check if left lane id is in the same segment
const auto adj_lane_it = std::find_if(lanes.begin(), lanes.end(), [&lane](const Lane& lane_it) {
return lane.left_lane_id.value() == lane_it.id;
});
if (adj_lane_it == lanes.end()) {
// Error.
const std::string msg{"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a left lane id (" +
lane.left_lane_id.value() + ") that is not part of the segment " + segment.first +
"."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
evaluate(true,
{"Adjacent lane isn't part of the segment: Lane " + lane.id + " has a left lane id (" +
lane.left_lane_id.value() + ") that is not part of the segment " + segment.first + "."},
Validator::Error::Type::kLogicalLaneAdjacency);
} else {
// Check if left lane id has the lane id as right lane id.
if (adj_lane_it->right_lane_id) {
if (adj_lane_it->right_lane_id.value() != lane.id) {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" +
lane.left_lane_id.value() + ") that has a right lane id (" +
adj_lane_it->right_lane_id.value() + ") that is not the lane " + lane.id + "."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
}
evaluate(adj_lane_it->right_lane_id.value() != lane.id,
{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" +
lane.left_lane_id.value() + ") that has a right lane id (" +
adj_lane_it->right_lane_id.value() + ") that is not the lane " + lane.id + "."},
Validator::Error::Type::kLogicalLaneAdjacency);
} else {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" +
lane.left_lane_id.value() + ") that has no right lane id."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
evaluate(true,
{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" +
lane.left_lane_id.value() + ") that has no right lane id."},
Validator::Error::Type::kLogicalLaneAdjacency);
}

// Check geometrical adjacency.
CheckAdjacency(lane, *adj_lane_it, kLeft, config.linear_tolerance);
evaluate(!AreAdjacent(lane, *adj_lane_it, kLeft, config.linear_tolerance),
{"Lane " + lane.id + " and lane " + adj_lane_it->id + " are not adjacent under the tolerance " +
std::to_string(config.linear_tolerance) + "."},
Validator::Error::Type::kGeometricalLaneAdjacency);
}

// Check if idx + 1 lane is the left lane id.
if ((idx + 1 <= static_cast<int>(lanes.size()) - 1) && (lanes[idx + 1].id != lane.left_lane_id.value())) {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" +
lane.left_lane_id.value() + ") that is not the next lane in the segment " +
segment.first + "."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
evaluate(true,
{"Wrong ordering of lanes: Lane " + lane.id + " has a left lane id (" + lane.left_lane_id.value() +
") that is not the next lane in the segment " + segment.first + "."},
Validator::Error::Type::kLogicalLaneAdjacency);
}

} else {
// Check if idx is the last lane in the segment.
if (idx != static_cast<int>(lanes.size()) - 1) {
// Error.
const std::string msg{"Wrong ordering of lanes: Lane " + lane.id +
" has no left lane id but it isn't the last lane in the segment " + segment.first +
"."};
Report(msg, Validator::Error::Type::kLaneAdjacency, Validator::Error::Severity::kError);
}
evaluate(idx != static_cast<int>(lanes.size()) - 1,
{"Wrong ordering of lanes: Lane " + lane.id +
" has no left lane id but it isn't the last lane in the segment " + segment.first + "."},
Validator::Error::Type::kLogicalLaneAdjacency);
}
}
}
}
return errors;
}

bool Validator::Error::operator==(const Error& other) const {
Expand Down