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(map): add differential lanelet loading #6241

Open
wants to merge 18 commits into
base: main
Choose a base branch
from
+1,093 −8
Open

feat(map): add differential lanelet loading #6241

Changes from 1 commit
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
690d608
feat(map): add differential lanelet loading
Aug 26, 2024
aafe0c2
fix: json schema check
Aug 26, 2024
9ae9485
fix: add region check
Aug 27, 2024
beb7faf
chore: make `enable_differential_loading` default `true`
Sep 3, 2024
af9ec44
fix: json schema check
Sep 3, 2024
999e023
chore: update maintainers
Sep 5, 2024
8308c18
chore: update map_loader documentation
Sep 7, 2024
585efca
style(pre-commit): autofix
pre-commit-ci[bot] Sep 7, 2024
4bbefcb
chore: autoware_dynamic_lanelet_provider `README.md`
Sep 9, 2024
681b887
chore: update map_loader `README.md`
Sep 9, 2024
42baae0
Merge branch 'main' into feat/add_dynamic_lanelet_loading
YamatoAndo Sep 24, 2024
9d9d75c
Merge branch 'main' into feat/add_dynamic_lanelet_loading
YamatoAndo Sep 26, 2024
6232b70
Update dependencies
StepTurtle Sep 26, 2024
28ad048
style(pre-commit): autofix
pre-commit-ci[bot] Sep 26, 2024
0fc1e77
fix: wrong msg type in README.md
StepTurtle Sep 27, 2024
65e382c
chore: remove unused dep.
StepTurtle Sep 27, 2024
2ad7615
style(pre-commit): autofix
pre-commit-ci[bot] Sep 27, 2024
5000421
Merge branch 'main' into feat/add_dynamic_lanelet_loading
YamatoAndo Sep 30, 2024
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
Prev Previous commit
Next Next commit
Loading
Loading status checks…
style(pre-commit): autofix
@pre-commit-ci
pre-commit-ci[bot] committed Sep 7, 2024
commit 585efcae15152aa6b76c8cd98fe82abdd9b4af8d
1 change: 0 additions & 1 deletion map/map_loader/README.md
View file
Original file line number Diff line number Diff line change
@@ -165,7 +165,6 @@ C.osm: [1200, 2520] # -> 1200 < x < 1220, 2520 < y < 2540
D.osm: [1240, 2520] # -> 1240 < x < 1260, 2520 < y < 2540
```


where,

- `x_resolution` and `y_resolution` are the resolution of the map in meters.

Unchanged files with check annotations Beta

View file
map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp
return std::hypot(dx, dy);
}
template <typename T>
bool is_inside_region(
const double & min_x, const double & max_x, const double & min_y, const double & max_y,
const T & point)
{
return min_x <= getX(point) && getX(point) <= max_x && min_y <= getY(point) &&
getY(point) <= max_y;
}

Check warning on line 63 in map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Excess Number of Function Arguments 

is_inside_region has 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
DynamicLaneletProviderNode::DynamicLaneletProviderNode(const rclcpp::NodeOptions & options)
: Node("dynamic_lanelet_provider", options),
}
}
void DynamicLaneletProviderNode::updateMap(const geometry_msgs::msg::Point & pose)
{
std::vector<std::string> cache_ids;
for (const auto & metadata : lanelet_map_meta_data_list_) {
geometry_msgs::msg::Point point;
point.x = (metadata.min_x + metadata.max_x) / 2;
point.y = (metadata.min_y + metadata.max_y) / 2;
if (is_inside_region(metadata.min_x, metadata.max_x, metadata.min_y, metadata.max_y, pose)) {
cache_ids.push_back(metadata.id);
continue;
}
double distance = norm_xy(point, pose);
if (distance < dynamic_map_loading_map_radius_) {
cache_ids.push_back(metadata.id);
}
}
if (cache_ids.empty()) {
RCLCPP_ERROR(get_logger(), "No lanelet map is found in the radius.");
return;
}
auto request = std::make_shared<autoware_map_msgs::srv::GetSelectedLanelet2Map::Request>();
request->cell_ids.insert(request->cell_ids.end(), cache_ids.begin(), cache_ids.end());
while (!map_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
RCLCPP_INFO(get_logger(), "Waiting for lanelet loader service");
}
auto result{map_loader_client_->async_send_request(
request, [](rclcpp::Client<autoware_map_msgs::srv::GetSelectedLanelet2Map>::SharedFuture) {})};
std::future_status status = result.wait_for(std::chrono::seconds(0));
while (status != std::future_status::ready) {
switch (status) {
case std::future_status::ready:
RCLCPP_INFO(get_logger(), "The future status is (ready).");
break;
case std::future_status::timeout:
RCLCPP_INFO(get_logger(), "The future status is (timed out).");
break;
case std::future_status::deferred:
RCLCPP_INFO(get_logger(), "The future status is (deferred).");
break;
}
RCLCPP_INFO(get_logger(), "waiting response from lanelet loader service.");
if (!rclcpp::ok()) {
return;
}
status = result.wait_for(std::chrono::seconds(1));
}
dynamic_map_pub_->publish(result.get()->lanelet2_cells);
}

Check warning on line 188 in map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

DynamicLaneletProviderNode::updateMap has a cyclomatic complexity of 13, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 188 in map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

DynamicLaneletProviderNode::updateMap has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
void DynamicLaneletProviderNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr & msg)
{
View file
map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp
return true;
}
lanelet::LaneletMapPtr Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
std::vector<std::string> & lanelet2_paths)
{
if (projector_info_.value().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
std::unique_ptr<lanelet::Projector> projector =
autoware::geography_utils::get_lanelet2_projector(projector_info_.value());
lanelet::ErrorMessages errors{};
lanelet::io_handlers::MultiOsmParser parser(*projector);
lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
if (!errors.empty()) {
for (const auto & error : errors) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
}
}
return map;
} else {
const lanelet::projection::LocalProjector projector;
lanelet::ErrorMessages errors{};
lanelet::io_handlers::MultiOsmParser parser(projector);
lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
if (!errors.empty()) {
for (const auto & error : errors) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
}
}
// overwrite local_x, local_y
for (lanelet::Point3d point : map->pointLayer) {
if (point.hasAttribute("local_x")) {
point.x() = point.attribute("local_x").asDouble().value();
}
if (point.hasAttribute("local_y")) {
point.y() = point.attribute("local_y").asDouble().value();
}
}
// realign lanelet borders using updated points
for (lanelet::Lanelet lanelet : map->laneletLayer) {
auto left = lanelet.leftBound();
auto right = lanelet.rightBound();
std::tie(left, right) = lanelet::geometry::align(left, right);
lanelet.setLeftBound(left);
lanelet.setRightBound(right);
}
return map;
}
}

Check warning on line 119 in map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

Lanelet2DifferentialLoaderModule::differentialLanelet2Load has a cyclomatic complexity of 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 119 in map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

Lanelet2DifferentialLoaderModule::differentialLanelet2Load has 3 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
void Lanelet2DifferentialLoaderModule::setLaneletMapMetadata(
std::map<std::string, Lanelet2FileMetaData> & lanelet2_metadata_dict, double x_res, double y_res)
View file
map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
// Copyright 2021 TierIV

Check warning on line 1 in map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Overall Code Complexity 

This module has a mean cyclomatic complexity of 5.50 across 8 functions. The mean complexity threshold is 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
const MapProjectorInfo::Message::ConstSharedPtr msg)
{
const auto allow_unsupported_version = get_parameter("allow_unsupported_version").as_bool();
auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string();
const auto center_line_resolution = get_parameter("center_line_resolution").as_double();
const auto use_waypoints = get_parameter("use_waypoints").as_bool();
const auto enable_differential_map_loading =
get_parameter("enable_differential_map_loading").as_bool();
lanelet::LaneletMapPtr map;
if (!enable_differential_map_loading) {
map = load_map(lanelet2_filename, *msg);
if (!map) {
RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published.");
return;
}
} else {
RCLCPP_INFO(get_logger(), "Differential lanelet2 map loading is enabled.");
std::vector<std::string> lanelet2_paths_or_directory = {
get_parameter("lanelet2_map_path").as_string()};
std::string lanelet2_metadata_path =
declare_parameter<std::string>("lanelet2_map_metadata_path");
double x_resolution, y_resolution;
std::map<std::string, Lanelet2FileMetaData> lanelet2_metadata_dict;
if (std::filesystem::exists(lanelet2_metadata_path)) {
lanelet2_metadata_dict = get_lanelet2_metadata(
lanelet2_metadata_path, get_lanelet2_paths(lanelet2_paths_or_directory), x_resolution,
y_resolution);
} else {
if (lanelet2_paths_or_directory.size() == 1) {
// Create a dummy metadata for a single osm file
lanelet2_metadata_dict =
get_dummy_lanelet2_metadata(lanelet2_filename, msg, x_resolution, y_resolution);
} else {
throw std::runtime_error("Lanelet2 metadata file not found: " + lanelet2_metadata_path);
}
}
{
// set metadata and projection info to differential loader module
differential_loader_module_->setLaneletMapMetadata(
lanelet2_metadata_dict, x_resolution, y_resolution);
differential_loader_module_->setProjectionInfo(*msg);
}
{
// load whole map for once
std::vector<std::string> lanelet2_paths;
lanelet2_paths.reserve(lanelet2_metadata_dict.size());
for (const auto & [lanelet2_path, _] : lanelet2_metadata_dict) {
lanelet2_paths.push_back(lanelet2_path);
}
map = differential_loader_module_->differentialLanelet2Load(lanelet2_paths);
}
lanelet2_filename =
lanelet2_metadata_dict.begin()
->first; // TODO(StepTurtle): find better way: `parseVersions` function read the osm
// file to get map version info, so `lanelet2_filename` should
// be osm file path, it changes `lanelet2_filename` hard coded.
// Because of this, `lanelet2_filename` can't be const in line 103.

Check warning on line 162 in map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

Lanelet2MapLoaderNode::on_map_projector_info increases in cyclomatic complexity from 12 to 17, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 162 in map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Bumpy Road Ahead 

Lanelet2MapLoaderNode::on_map_projector_info increases from 2 to 5 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
}
std::string format_version{"null"}, map_version{""};
return map_bin_msg;
}
std::vector<std::string> Lanelet2MapLoaderNode::get_lanelet2_paths(
const std::vector<std::string> & lanelet2_paths_or_directory) const
{
std::vector<std::string> lanelet2_paths;
for (const auto & path : lanelet2_paths_or_directory) {
if (!std::filesystem::exists(path)) {
RCLCPP_ERROR_STREAM(get_logger(), "No such file or directory: " << path);
continue;
}
if (isOsmFile(path)) {
lanelet2_paths.push_back(path);
}
if (std::filesystem::is_directory(path)) {
for (const auto & file : std::filesystem::directory_iterator(path)) {
const auto filename = file.path().string();
if (isOsmFile(filename)) {
lanelet2_paths.push_back(filename);
}
}
}
}
return lanelet2_paths;
}

Check warning on line 303 in map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

Lanelet2MapLoaderNode::get_lanelet2_paths has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check warning on line 303 in map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Deep, Nested Complexity 

Lanelet2MapLoaderNode::get_lanelet2_paths has a nested complexity depth of 4, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
std::map<std::string, Lanelet2FileMetaData> Lanelet2MapLoaderNode::get_lanelet2_metadata(
const std::string & lanelet2_metadata_path, const std::vector<std::string> & lanelet2_paths,