#include <lanelet2_map_interface.hpp>

Public Member Functions
	Lanelet2MapInterface (rclcpp::Node &parent_node, std::string map_server_name)
	Creates a client-side interface for retrieving Lanelet2 map parameters from a map server node.

lanelet::LaneletMapConstPtr	getMapPtr ()
	Returns the currently loaded immutable Lanelet2 map.

lanelet::LaneletMapPtr	getNonConstMapPtr ()
	Returns the currently loaded mutable Lanelet2 map.

std::shared_ptr< lanelet::Projector >	getProjectorPtr ()
	Returns the projector used to interpret the loaded map coordinates.

Public Attributes
bool	map_loaded_ = false

bool	update_pending_ = false

std::string	map_frame_id_

Private Member Functions
void	updateMapParam (rclcpp::Parameter param)
	Updates one cached map parameter from a value received from the map server.

bool	loadMap ()
	Loads and parses the map from the currently cached parameters.

bool	validateParams ()
	Validates that the required map parameters are available and usable.

void	findMapServer ()
	Checks for the configured map server and, once available, requests initial parameters and installs subscriptions.

void	updateParamsCallback (const rclcpp::Parameter &p)
	Reacts to a single parameter event from the map server.

void	serviceParamsCallback (std::shared_future< std::vector< rclcpp::Parameter > > future)
	Handles the asynchronous response of an initial parameter fetch.

Private Attributes
rclcpp::Node *	parent_node_

std::shared_ptr< rclcpp::AsyncParametersClient >	parameter_client_

std::shared_ptr< rclcpp::ParameterEventHandler >	parameter_sub_

std::shared_ptr< rclcpp::ParameterCallbackHandle >	frame_id_callback_handle_

std::shared_ptr< rclcpp::ParameterCallbackHandle >	contents_callback_handle_

std::shared_ptr< rclcpp::ParameterCallbackHandle >	origin_lat_callback_handle_

std::shared_ptr< rclcpp::ParameterCallbackHandle >	origin_lon_callback_handle_

rclcpp::TimerBase::SharedPtr	startup_timer_

std::string	map_filepath_

std::vector< rclcpp::Parameter >	map_params_

std::string	map_contents_

double	origin_lat_ = std::numeric_limits<double>::quiet_NaN()

double	origin_lon_ = std::numeric_limits<double>::quiet_NaN()

lanelet::LaneletMapPtr	mapPtr_

std::shared_ptr< lanelet::Projector >	utmProjectorPtr_

std::string	map_server_name_

bool	params_declared_ = false

Detailed Description

Definition at line 17 of file lanelet2_map_interface.hpp.

Constructor & Destructor Documentation

◆ Lanelet2MapInterface()

Lanelet2MapInterface::Lanelet2MapInterface	(	rclcpp::Node &	parent_node,
		std::string	map_server_name )

Creates a client-side interface for retrieving Lanelet2 map parameters from a map server node.

Parameters

parent_node	node that owns the parameter client and timers
map_server_name	fully qualified name of the map server node

Definition at line 41 of file lanelet2_map_interface.cpp.

    : parent_node_(&parent_node),
      map_filepath_(std::string(".lanelet2_map_interface/") + std::string(parent_node.get_fully_qualified_name()) + "/map.osm"),
      map_server_name_(map_server_name) {
  rcl_interfaces::msg::ParameterDescriptor param_desc;
  param_desc.description = "Local filepath to where map from map server is written to (relative to $ROS_HOME)";
  try {
    parent_node_->declare_parameter("map_filepath", map_filepath_, param_desc);
  } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException& ex) {
    RCLCPP_WARN(parent_node_->get_logger(), "Parameter 'map_filepath' already declared: %s", ex.what());
  }
  map_filepath_ = parent_node_->get_parameter("map_filepath").as_string();
  map_filepath_ = resolveFilepath(map_filepath_).string();
  RCLCPP_INFO(parent_node_->get_logger(), "Loaded parameter 'map_filepath': %s", map_filepath_.c_str());
 
  // Initialize parameter client and event handler
  parameter_client_ = std::make_shared<rclcpp::AsyncParametersClient>(parent_node_, map_server_name);
  parameter_sub_ = std::make_shared<rclcpp::ParameterEventHandler>(parent_node_);
 
  // Periodically check if the parameter server is available
  startup_timer_ =
      parent_node_->create_wall_timer(std::chrono::seconds(1), std::bind(&Lanelet2MapInterface::findMapServer, this));
}

Member Function Documentation

◆ findMapServer()

void Lanelet2MapInterface::findMapServer ( )

private

Checks for the configured map server and, once available, requests initial parameters and installs subscriptions.

Definition at line 65 of file lanelet2_map_interface.cpp.

                                         {
  if (!parameter_client_->wait_for_service(std::chrono::milliseconds(10))) {
    RCLCPP_WARN(parent_node_->get_logger(), "Waiting for map server ('%s') parameter service ...", map_server_name_.c_str());
    return;
  }
 
  startup_timer_->cancel();
  auto parameters_future =
      parameter_client_->get_parameters({"map_frame_id", "map_contents", "origin_lat", "origin_lon"},
                                        std::bind(&Lanelet2MapInterface::serviceParamsCallback, this, std::placeholders::_1));
  RCLCPP_INFO(parent_node_->get_logger(), "Connected to map server ('%s') parameter service", map_server_name_.c_str());
 
  // Only declare parameters once
  if (!params_declared_) {
    frame_id_callback_handle_ = parameter_sub_->add_parameter_callback(
        "map_frame_id", std::bind(&Lanelet2MapInterface::updateParamsCallback, this, std::placeholders::_1), map_server_name_);
    contents_callback_handle_ = parameter_sub_->add_parameter_callback(
        "map_contents", std::bind(&Lanelet2MapInterface::updateParamsCallback, this, std::placeholders::_1), map_server_name_);
    origin_lat_callback_handle_ = parameter_sub_->add_parameter_callback(
        "origin_lat", std::bind(&Lanelet2MapInterface::updateParamsCallback, this, std::placeholders::_1), map_server_name_);
    origin_lon_callback_handle_ = parameter_sub_->add_parameter_callback(
        "origin_lon", std::bind(&Lanelet2MapInterface::updateParamsCallback, this, std::placeholders::_1), map_server_name_);
    params_declared_ = true;
  }
}

◆ getMapPtr()

lanelet::LaneletMapConstPtr Lanelet2MapInterface::getMapPtr ( )

Returns the currently loaded immutable Lanelet2 map.

Returns: shared pointer to the loaded map, or nullptr if no map is available

Definition at line 136 of file lanelet2_map_interface.cpp.

136{ return getNonConstMapPtr(); }

Lanelet2MapInterface::getNonConstMapPtr

lanelet::LaneletMapPtr getNonConstMapPtr()

Returns the currently loaded mutable Lanelet2 map.

Definition lanelet2_map_interface.cpp:129

◆ getNonConstMapPtr()

lanelet::LaneletMapPtr Lanelet2MapInterface::getNonConstMapPtr ( )

Returns the currently loaded mutable Lanelet2 map.

Returns: shared pointer to the loaded map, or nullptr if no map is available

Definition at line 129 of file lanelet2_map_interface.cpp.

                                                             {
  if (!map_loaded_) {
    return nullptr;
  }
  return mapPtr_;
}

◆ getProjectorPtr()

std::shared_ptr< lanelet::Projector > Lanelet2MapInterface::getProjectorPtr ( )

Returns the projector used to interpret the loaded map coordinates.

Returns: projector configured from the map origin parameters

Definition at line 138 of file lanelet2_map_interface.cpp.

                                                                      {
  if (!map_loaded_) {
    return nullptr;
  }
  return utmProjectorPtr_;
}

◆ loadMap()

bool Lanelet2MapInterface::loadMap ( )

private

Loads and parses the map from the currently cached parameters.

Returns: true if the map could be created successfully

Definition at line 160 of file lanelet2_map_interface.cpp.

                                   {
  // Validate parameters
  if (!validateParams()) {
    return false;
  }
 
  // Check if location of map file exists, if not create all folders
  std::string map_directory = map_filepath_.substr(0, map_filepath_.find_last_of("/"));
  if (!std::filesystem::exists(map_directory)) {
    if (!std::filesystem::create_directories(map_directory)) {
      RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "Failed to create directory '" << map_directory << "'");
      return false;
    }
  }
 
  // Save map_contents to file
  std::ofstream map_file;
  map_file.open(map_filepath_);
  map_file << map_contents_;
  map_file.close();
 
  try {
    // Load map from file
    utmProjectorPtr_ = std::make_shared<lanelet::projection::UtmProjector>(lanelet::Origin({origin_lat_, origin_lon_}));
    mapPtr_ = lanelet::load(map_filepath_, *utmProjectorPtr_);
    map_loaded_ = true;
    RCLCPP_INFO_STREAM(parent_node_->get_logger(), "Loaded map '" + map_filepath_ + "'");
    update_pending_ = true;
  } catch (const std::exception& exc) {
    RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "Failed to load map '" + map_filepath_ + "': " + exc.what());
    return false;
  }
 
  return true;
}

◆ serviceParamsCallback()

void Lanelet2MapInterface::serviceParamsCallback ( std::shared_future< std::vector< rclcpp::Parameter > > future )

private

Handles the asynchronous response of an initial parameter fetch.

Parameters

future future containing the requested map server parameters

Definition at line 97 of file lanelet2_map_interface.cpp.

                                                                                                    {
  auto result = future.get();
  for (auto& parameter : result) {
    updateMapParam(parameter);
  }
  std::ignore = loadMap();
}

◆ updateMapParam()

void Lanelet2MapInterface::updateMapParam ( rclcpp::Parameter param )

private

Updates one cached map parameter from a value received from the map server.

Parameters

param updated parameter value from the map server

Definition at line 145 of file lanelet2_map_interface.cpp.

                                                               {
  if (param.get_name() == "map_frame_id") {
    map_frame_id_ = param.value_to_string();
  }
  if (param.get_name() == "map_contents") {
    map_contents_ = param.value_to_string();
  }
  if (param.get_name() == "origin_lat") {
    origin_lat_ = param.as_double();
  }
  if (param.get_name() == "origin_lon") {
    origin_lon_ = param.as_double();
  }
}

◆ updateParamsCallback()

void Lanelet2MapInterface::updateParamsCallback ( const rclcpp::Parameter & p )

private

Reacts to a single parameter event from the map server.

Parameters

p	parameter that changed on the server

Definition at line 91 of file lanelet2_map_interface.cpp.

                                                                        {
  RCLCPP_INFO_STREAM(parent_node_->get_logger(), "Parameter '" << p.get_name() << "' changed, reloading map");
  updateMapParam(p);
  std::ignore = loadMap();
}

◆ validateParams()

bool Lanelet2MapInterface::validateParams ( )

private

Validates that the required map parameters are available and usable.

Returns: true if the cached parameters are sufficient for loading a map

Definition at line 105 of file lanelet2_map_interface.cpp.

                                          {
  if (map_frame_id_.empty()) {
    RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "Parameter 'map_frame_id_' is empty");
    return false;
  }
  if (map_filepath_.empty()) {
    RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "Parameter 'map_filepath_' is empty");
    return false;
  }
  if (map_contents_.empty()) {
    RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "Parameter 'map_contents_' is empty");
    return false;
  }
  if (origin_lat_ == std::numeric_limits<double>::quiet_NaN()) {
    RCLCPP_WARN_STREAM(parent_node_->get_logger(), "Parameter 'origin_lat_' is not set");
    return false;
  }
  if (origin_lon_ == std::numeric_limits<double>::quiet_NaN()) {
    RCLCPP_WARN_STREAM(parent_node_->get_logger(), "Parameter 'origin_lon_' is not set");
    return false;
  }
  return true;
}

Member Data Documentation

◆ contents_callback_handle_

std::shared_ptr<rclcpp::ParameterCallbackHandle> Lanelet2MapInterface::contents_callback_handle_

private

Definition at line 55 of file lanelet2_map_interface.hpp.

◆ frame_id_callback_handle_

std::shared_ptr<rclcpp::ParameterCallbackHandle> Lanelet2MapInterface::frame_id_callback_handle_

private

Definition at line 55 of file lanelet2_map_interface.hpp.

◆ map_contents_

std::string Lanelet2MapInterface::map_contents_

private

Definition at line 62 of file lanelet2_map_interface.hpp.

◆ map_filepath_

std::string Lanelet2MapInterface::map_filepath_

private

Definition at line 59 of file lanelet2_map_interface.hpp.

◆ map_frame_id_

std::string Lanelet2MapInterface::map_frame_id_

Definition at line 49 of file lanelet2_map_interface.hpp.

◆ map_loaded_

bool Lanelet2MapInterface::map_loaded_ = false

Definition at line 47 of file lanelet2_map_interface.hpp.

◆ map_params_

std::vector<rclcpp::Parameter> Lanelet2MapInterface::map_params_

private

Definition at line 61 of file lanelet2_map_interface.hpp.

◆ map_server_name_

std::string Lanelet2MapInterface::map_server_name_

private

Definition at line 69 of file lanelet2_map_interface.hpp.

◆ mapPtr_

lanelet::LaneletMapPtr Lanelet2MapInterface::mapPtr_

private

Definition at line 66 of file lanelet2_map_interface.hpp.

◆ origin_lat_

double Lanelet2MapInterface::origin_lat_ = std::numeric_limits<double>::quiet_NaN()

private

Definition at line 63 of file lanelet2_map_interface.hpp.

◆ origin_lat_callback_handle_

std::shared_ptr<rclcpp::ParameterCallbackHandle> Lanelet2MapInterface::origin_lat_callback_handle_

private

Definition at line 56 of file lanelet2_map_interface.hpp.

◆ origin_lon_

double Lanelet2MapInterface::origin_lon_ = std::numeric_limits<double>::quiet_NaN()

private

Definition at line 64 of file lanelet2_map_interface.hpp.

◆ origin_lon_callback_handle_

std::shared_ptr<rclcpp::ParameterCallbackHandle> Lanelet2MapInterface::origin_lon_callback_handle_

private

Definition at line 56 of file lanelet2_map_interface.hpp.

◆ parameter_client_

std::shared_ptr<rclcpp::AsyncParametersClient> Lanelet2MapInterface::parameter_client_

private

Definition at line 53 of file lanelet2_map_interface.hpp.

◆ parameter_sub_

std::shared_ptr<rclcpp::ParameterEventHandler> Lanelet2MapInterface::parameter_sub_

private

Definition at line 54 of file lanelet2_map_interface.hpp.

◆ params_declared_

bool Lanelet2MapInterface::params_declared_ = false

private

Definition at line 70 of file lanelet2_map_interface.hpp.

◆ parent_node_

rclcpp::Node* Lanelet2MapInterface::parent_node_

private

Definition at line 52 of file lanelet2_map_interface.hpp.

◆ startup_timer_

rclcpp::TimerBase::SharedPtr Lanelet2MapInterface::startup_timer_

private

Definition at line 57 of file lanelet2_map_interface.hpp.

◆ update_pending_

bool Lanelet2MapInterface::update_pending_ = false

Definition at line 48 of file lanelet2_map_interface.hpp.

◆ utmProjectorPtr_

std::shared_ptr<lanelet::Projector> Lanelet2MapInterface::utmProjectorPtr_

private

Definition at line 67 of file lanelet2_map_interface.hpp.

The documentation for this class was generated from the following files:

lanelet2_map_interface/include/lanelet2_map_interface/lanelet2_map_interface.hpp
lanelet2_map_interface/src/lanelet2_map_interface.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Lanelet2MapInterface()

Member Function Documentation

◆ findMapServer()

◆ getMapPtr()

◆ getNonConstMapPtr()

◆ getProjectorPtr()

◆ loadMap()

◆ serviceParamsCallback()

◆ updateMapParam()

◆ updateParamsCallback()

◆ validateParams()

Member Data Documentation

◆ contents_callback_handle_

◆ frame_id_callback_handle_

◆ map_contents_

◆ map_filepath_

◆ map_frame_id_

◆ map_loaded_

◆ map_params_

◆ map_server_name_

◆ mapPtr_

◆ origin_lat_

◆ origin_lat_callback_handle_

◆ origin_lon_

◆ origin_lon_callback_handle_

◆ parameter_client_

◆ parameter_sub_

◆ params_declared_

◆ parent_node_

◆ startup_timer_

◆ update_pending_

◆ utmProjectorPtr_