Skip to content

Commit

Permalink
Update
Browse files Browse the repository at this point in the history 
Signed-off-by: Barış Zeren <[email protected]>
  • Loading branch information
@StepTurtle
StepTurtle committed Feb 28, 2024
1 parent 4572933 commit e02348d
Show file tree
Hide file tree
Showing 10 changed files with 149 additions and 76 deletions.
2 changes: 0 additions & 2 deletions launch/tier4_map_launch/launch/map.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
<arg name="pointcloud_map_path"/>
<arg name="pointcloud_map_metadata_path"/>
<arg name="lanelet2_map_path"/>
<arg name="lanelet2_map_folder_path"/>
<arg name="lanelet2_map_metadata_path"/>
<arg name="map_projector_info_path"/>

Expand Down Expand Up @@ -41,7 +40,6 @@
<composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader">
<param from="$(var lanelet2_map_loader_param_path)"/>
<param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
<param name="lanelet2_map_folder_path" value="[$(var lanelet2_map_folder_path)]"/>
<param name="lanelet2_map_metadata_path" value="$(var lanelet2_map_metadata_path)"/>
<remap from="output/lanelet2_map" to="vector_map"/>
<remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
Expand Down
2 changes: 1 addition & 1 deletion map/autoware_dynamic_lanelet_provider/config/dynamic_lanelet_provider.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,4 +2,4 @@
ros__parameters:
dynamic_map_loading_grid_size: 50.0 # [m]
dynamic_map_loading_update_distance: 100.0 # [m]
dynamic_map_loading_map_radius: 150.0 # [m]
dynamic_map_loading_map_radius: 200.0 # [m]
12 changes: 4 additions & 8 deletions ...c_lanelet_provider/include/autoware/dynamic_lanelet_provider/dynamic_lanelet_provider.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,10 @@ namespace dynamic_lanelet_provider
struct Lanelet2FileMetaData
{
int id;
double origin_lat;
double origin_lon;
std::string mgrs_code;
double min_x;
double max_x;
double min_y;
double max_y;
};

class DynamicLaneletProviderNode : public rclcpp::Node
Expand All @@ -47,9 +48,6 @@ class DynamicLaneletProviderNode : public rclcpp::Node

rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;

component_interface_utils::Subscription<map_interface::MapProjectorInfo>::SharedPtr
sub_map_projector_info_;

component_interface_utils::Subscription<map_interface::LaneletMapMetaData>::SharedPtr
lanelet_map_meta_data_sub_;

Expand All @@ -68,8 +66,6 @@ class DynamicLaneletProviderNode : public rclcpp::Node
const double dynamic_map_loading_map_radius_;

std::vector<Lanelet2FileMetaData> lanelet_map_meta_data_list_;

std::optional<tier4_map_msgs::msg::MapProjectorInfo> projector_info_;
};
} // namespace dynamic_lanelet_provider
} // namespace autoware
Expand Down
43 changes: 14 additions & 29 deletions map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,23 +63,17 @@ DynamicLaneletProviderNode::DynamicLaneletProviderNode(const rclcpp::NodeOptions
"input/odometry", 100,
std::bind(&DynamicLaneletProviderNode::onOdometry, this, std::placeholders::_1));

const auto projector_info_adaptor = component_interface_utils::NodeAdaptor(this);
projector_info_adaptor.init_sub(
sub_map_projector_info_,
[this](const map_interface::MapProjectorInfo::Message::ConstSharedPtr msg) {
projector_info_ = *msg;
});

const auto metadata_adaptor = component_interface_utils::NodeAdaptor(this);
metadata_adaptor.init_sub(
lanelet_map_meta_data_sub_,
[this](const autoware_map_msgs::msg::LaneletMapMetaData::SharedPtr msg) {
for (const auto & data : msg->metadata_list) {
Lanelet2FileMetaData metadata;
metadata.id = data.tile_id;
metadata.origin_lat = data.origin_lat;
metadata.origin_lon = data.origin_lon;
metadata.mgrs_code = data.mgrs_code;
metadata.min_x = data.min_x;
metadata.max_x = data.max_x;
metadata.min_y = data.min_y;
metadata.max_y = data.max_y;
lanelet_map_meta_data_list_.push_back(metadata);
}
});
Expand All @@ -102,8 +96,9 @@ void DynamicLaneletProviderNode::mapUpdateTimerCallback()
return;
}

if (projector_info_ == std::nullopt || lanelet_map_meta_data_list_.empty()) {
if (lanelet_map_meta_data_list_.empty()) {
RCLCPP_ERROR_ONCE(get_logger(), "Check your lanelet map metadata and projector info.");
return;
}

if (should_update_map()) {
Expand All @@ -117,25 +112,15 @@ void DynamicLaneletProviderNode::mapUpdateTimerCallback()
void DynamicLaneletProviderNode::updateMap(const geometry_msgs::msg::Point & pose)
{
std::vector<int> cache_ids;
if (projector_info_.value().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
std::unique_ptr<lanelet::Projector> projector =
geography_utils::get_lanelet2_projector(projector_info_.value());

for (const auto & metadata : lanelet_map_meta_data_list_) {
lanelet::GPSPoint gps_point;
gps_point.lat = metadata.origin_lat;
gps_point.lon = metadata.origin_lon;
gps_point.ele = 0;

lanelet::BasicPoint3d point = projector->forward(gps_point);

double distance = norm_xy(point, pose);
if (distance < dynamic_map_loading_map_radius_) {
cache_ids.push_back(metadata.id);
}
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;

double distance = norm_xy(point, pose);
if (distance < dynamic_map_loading_map_radius_) {
cache_ids.push_back(metadata.id);
}
} else {
// TODO: cannot reach LocalProjector here
}

if (cache_ids.empty()) {
Expand Down
10 changes: 8 additions & 2 deletions map/map_loader/include/map_loader/lanelet2_differential_loader_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,12 +48,15 @@ class Lanelet2DifferentialLoaderModule
public:
explicit Lanelet2DifferentialLoaderModule(
rclcpp::Node * node,
const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict);
const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict, double & x_res,
double & y_res);

private:
rclcpp::Logger logger_;
rclcpp::Clock::SharedPtr clock_;

rclcpp::Publisher<HADMapBin>::SharedPtr pub_whole_map_bin_;

std::map<std::string, Lanelet2FileMetaData> lanelet2_file_metadata_dict_;

rclcpp::Service<GetDifferentialLanelet2Map>::SharedPtr get_differential_lanelet2_maps_service_;
Expand All @@ -70,11 +73,14 @@ class Lanelet2DifferentialLoaderModule
GetDifferentialLanelet2Map::Request::SharedPtr req,
GetDifferentialLanelet2Map::Response::SharedPtr res);

autoware_auto_mapping_msgs::msg::HADMapBin loadWholeMap();

bool differentialLanelet2Load(
GetDifferentialLanelet2Map::Request::SharedPtr & request,
GetDifferentialLanelet2Map::Response::SharedPtr & response);

autoware_map_msgs::msg::LaneletMapMetaData getLaneletMapMetaDataMsg() const;
autoware_map_msgs::msg::LaneletMapMetaData getLaneletMapMetaDataMsg(
const double & x_res, const double & y_res) const;
};

#endif // MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_
4 changes: 2 additions & 2 deletions map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,8 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
std::vector<std::string> getLanelet2Paths(
const std::vector<std::string> & lanelet2_paths_or_directory) const;
std::map<std::string, Lanelet2FileMetaData> getLanelet2Metadata(
const std::string & lanelet2_metadata_path,
const std::vector<std::string> & lanelet2_paths) const;
const std::string & lanelet2_metadata_path, const std::vector<std::string> & lanelet2_paths,
double & x_resolution, double & y_resolution) const;
};

#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
3 changes: 1 addition & 2 deletions map/map_loader/include/map_loader/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,10 @@ struct Lanelet2FileMetaData
int id;
double origin_lat;
double origin_lon;
std::string mgrs_code;
};

std::map<std::string, Lanelet2FileMetaData> loadLanelet2Metadata(
const std::string & lanelet2_metadata_path);
const std::string & lanelet2_metadata_path, double & x_resolution, double & y_resolution);
std::map<std::string, Lanelet2FileMetaData> replaceWithAbsolutePath(
const std::map<std::string, Lanelet2FileMetaData> & lanelet2_metadata_path,
const std::vector<std::string> & lanelet2_paths);
Expand Down
102 changes: 89 additions & 13 deletions map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,25 +19,35 @@

Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule(
rclcpp::Node * node,
const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict)
const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict, double & x_res,
double & y_res)
: logger_(node->get_logger()),
clock_(node->get_clock()),
lanelet2_file_metadata_dict_(lanelet2_file_metadata_dict)
{
// Publish lanelet2 map meta data
{
const auto msg = getLaneletMapMetaDataMsg();
pub_whole_map_bin_ =
node->create_publisher<HADMapBin>("output/lanelet2_map", rclcpp::QoS{1}.transient_local());

const auto metadata_adaptor = component_interface_utils::NodeAdaptor(node);
metadata_adaptor.init_pub(pub_lanelet_map_meta_data_);
pub_lanelet_map_meta_data_->publish(msg);
}
const auto metadata_adaptor = component_interface_utils::NodeAdaptor(node);
metadata_adaptor.init_pub(pub_lanelet_map_meta_data_);

const auto adaptor = component_interface_utils::NodeAdaptor(node);
adaptor.init_sub(
sub_map_projector_info_,
[this](const map_interface::MapProjectorInfo::Message::ConstSharedPtr msg) {
[this, &x_res, &y_res](const map_interface::MapProjectorInfo::Message::ConstSharedPtr msg) {
projector_info_ = *msg;

// load whole map once when projector info is available
{
const auto map = loadWholeMap();
pub_whole_map_bin_->publish(map);
}

// publish lanelet2 map metadata when projector info is available
{
const auto msg = getLaneletMapMetaDataMsg(x_res, y_res);
pub_lanelet_map_meta_data_->publish(msg);
}
});

get_differential_lanelet2_maps_service_ = node->create_service<GetDifferentialLanelet2Map>(
Expand All @@ -60,6 +70,57 @@ bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map(
return true;
}

autoware_auto_mapping_msgs::msg::HADMapBin Lanelet2DifferentialLoaderModule::loadWholeMap()
{
std::vector<std::string> lanelet2_paths;
for (const auto & file : lanelet2_file_metadata_dict_) {
if (!std::filesystem::exists(file.first)) {
continue;
}
lanelet2_paths.push_back(file.first);
}

if (projector_info_.value().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
std::unique_ptr<lanelet::Projector> projector =
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);
}
}

lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);

const auto map_bin_msg =
Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());

return map_bin_msg;
} 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);
}
}

lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);

const auto map_bin_msg =
Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());

return map_bin_msg;
}
}

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

Lanelet2DifferentialLoaderModule::loadWholeMap has a cyclomatic complexity of 9, 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 122 in map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

Lanelet2DifferentialLoaderModule::loadWholeMap 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.

bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
GetDifferentialLanelet2Map::Request::SharedPtr & request,
GetDifferentialLanelet2Map::Response::SharedPtr & response)
Expand Down Expand Up @@ -126,15 +187,30 @@ bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
}

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

Lanelet2DifferentialLoaderModule::differentialLanelet2Load has a cyclomatic complexity of 12, 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 187 in map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

View check run for this annotation

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.

autoware_map_msgs::msg::LaneletMapMetaData
Lanelet2DifferentialLoaderModule::getLaneletMapMetaDataMsg() const
Lanelet2DifferentialLoaderModule::getLaneletMapMetaDataMsg(
const double & x_res, const double & y_res) const
{
std::unique_ptr<lanelet::Projector> projector;
if (projector_info_.value().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
projector = geography_utils::get_lanelet2_projector(projector_info_.value());
} else {
projector = std::make_unique<lanelet::projection::LocalProjector>();
}

autoware_map_msgs::msg::LaneletMapMetaData metadata;
for (const auto & file : lanelet2_file_metadata_dict_) {
lanelet::GPSPoint gps_point;
gps_point.lat = file.second.origin_lat;
gps_point.lon = file.second.origin_lon;
gps_point.ele = 0;
lanelet::BasicPoint3d point = projector->forward(gps_point);

autoware_map_msgs::msg::LaneletMapTileMetaData tile_msg;
tile_msg.tile_id = file.second.id;
tile_msg.mgrs_code = file.second.mgrs_code;
tile_msg.origin_lat = file.second.origin_lat;
tile_msg.origin_lon = file.second.origin_lon;
tile_msg.min_x = point.x();
tile_msg.min_y = point.y();
tile_msg.max_x = point.x() + x_res;
tile_msg.max_y = point.y() + y_res;

metadata.metadata_list.push_back(tile_msg);
}
Expand Down
35 changes: 20 additions & 15 deletions map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,31 +73,35 @@ bool isOsmFile(const std::string & f)
Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options)
: Node("lanelet2_map_loader", options)
{
const auto adaptor = component_interface_utils::NodeAdaptor(this);

// subscription
adaptor.init_sub(
sub_map_projector_info_,
[this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); });

declare_parameter<std::string>("lanelet2_map_path");
declare_parameter<double>("center_line_resolution");

if (declare_parameter<bool>("enabled_dynamic_lanelet_loading")) {
std::vector<std::string> lanelet2_paths_or_directory =
declare_parameter<std::vector<std::string>>("lanelet2_map_folder_path");
if (isOsmFile(get_parameter("lanelet2_map_path").as_string())) {
RCLCPP_INFO(get_logger(), "Single osm file provided. Dynamic loading is disabled.");

const auto adaptor = component_interface_utils::NodeAdaptor(this);
adaptor.init_sub(
sub_map_projector_info_,
[this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); });
} else {
RCLCPP_INFO(get_logger(), "Multiple osm file provided. Dynamic loading is enabled.");

std::vector<std::string> lanelet2_paths_or_directory = {
get_parameter("lanelet2_map_path").as_string()};
std::string lanelet2_map_metadata_path =
declare_parameter<std::string>("lanelet2_map_metadata_path");
double x_resolution, y_resolution;

std::map<std::string, Lanelet2FileMetaData> lanelet2_metadata_dict;
try {
lanelet2_metadata_dict = getLanelet2Metadata(
lanelet2_map_metadata_path, getLanelet2Paths(lanelet2_paths_or_directory));
lanelet2_map_metadata_path, getLanelet2Paths(lanelet2_paths_or_directory), x_resolution,
y_resolution);
} catch (std::runtime_error & e) {
RCLCPP_ERROR_STREAM(get_logger(), "Failed to load lanelet2 metadata");
}
differential_loader_module_ =
std::make_unique<Lanelet2DifferentialLoaderModule>(this, lanelet2_metadata_dict);
differential_loader_module_ = std::make_unique<Lanelet2DifferentialLoaderModule>(
this, lanelet2_metadata_dict, x_resolution, y_resolution);
}
}

Expand Down Expand Up @@ -221,14 +225,15 @@ std::vector<std::string> Lanelet2MapLoaderNode::getLanelet2Paths(
}

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

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

Lanelet2MapLoaderNode::getLanelet2Paths 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 225 in map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Deep, Nested Complexity 

Lanelet2MapLoaderNode::getLanelet2Paths 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::getLanelet2Metadata(
const std::string & lanelet2_metadata_path, const std::vector<std::string> & lanelet2_paths) const
const std::string & lanelet2_metadata_path, const std::vector<std::string> & lanelet2_paths,
double & x_resolution, double & y_resolution) const
{
std::map<std::string, Lanelet2FileMetaData> lanelet2_metadata_dict;
if (!std::filesystem::exists(lanelet2_metadata_path)) {
throw std::runtime_error("Lanelet2 metadata file not found: " + lanelet2_metadata_path);
}

lanelet2_metadata_dict = loadLanelet2Metadata(lanelet2_metadata_path);
lanelet2_metadata_dict = loadLanelet2Metadata(lanelet2_metadata_path, x_resolution, y_resolution);
lanelet2_metadata_dict = replaceWithAbsolutePath(lanelet2_metadata_dict, lanelet2_paths);
RCLCPP_INFO_STREAM(get_logger(), "Loaded Lanelet2 metadata: " << lanelet2_metadata_path);

Expand Down
Loading

0 comments on commit e02348d

Please sign in to comment.