Lanelet2MapServer Class Reference

#include <lanelet2_map_server.hpp>

Inheritance diagram for Lanelet2MapServer:

Public Member Functions
	Lanelet2MapServer ()
	Creates the Lanelet2 map server node and declares its parameters.

Private Member Functions
template<typename T >
bool	declareAndLoadParameter (const std::string &name, T &param, const std::string &description, const bool add_to_auto_reconfigurable_params=true, const bool is_required=false, const bool read_only=false, const std::optional< double > &from_value=std::nullopt, const std::optional< double > &to_value=std::nullopt, const std::optional< double > &step_value=std::nullopt, const std::string &additional_constraints="")
	Declares and loads a ROS parameter.
rcl_interfaces::msg::SetParametersResult	parametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Handles reconfiguration when a parameter value is changed.
void	loadMapContents ()
	Loads the configured map file and caches its serialized contents.
void	unsetMapParameters ()
	Clears map-specific parameters after an invalid or unloaded map state.
void	updateMapParameters ()
	Writes the current map state into this node's exported ROS parameters.
void	setup ()
	Sets up timers, callbacks, publishers, and initial map loading.
void	find_available_maps (const std::string &directory, std::vector< Lanelet2MapMeta > &maps) const
	Recursively scans a directory for available Lanelet2 map files.
void	derive_map_meta (Lanelet2MapMeta &map_meta) const
	Fills the derived metadata fields for a discovered map.
bool	deriveOriginFromMap (const std::string &map_filepath, double &origin_lat, double &origin_lon) const
	Derives a south-west origin latitude and longitude from a Lanelet2 map file.
bool	map_sanity_check (std::string map_filepath, double origin_lat, double origin_lon) const
	Tries to load a map with the provided origin to verify that it is usable.
void	pub_tf () const
	Publishes the static transform from the derived UTM frame to the map frame.
void	navSatFixCallback (const sensor_msgs::msg::NavSatFix::SharedPtr msg)
	Stores the latest GNSS fix for automatic map selection.
void	automaticMapUpdateTimerCallback ()
	Periodically reevaluates the best map when automatic selection is enabled.

Static Private Member Functions
static void	derive_utm_zone (const double latitude, const double longitude, int &zone, bool &northp)
	Derives the UTM zone and hemisphere from a geographic position.

Private Attributes
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > >	auto_reconfigurable_params_
	Auto-reconfigurable parameters for dynamic reconfiguration.
OnSetParametersCallbackHandle::SharedPtr	parameters_callback_
	Callback handle for dynamic parameter reconfiguration.
rclcpp::TimerBase::SharedPtr	one_shot_timer_
rclcpp::TimerBase::SharedPtr	automatic_map_timer_
bool	use_automatic_map_selection_ = false
std::string	map_directory_ = "/data/maps/default-maps"
std::vector< Lanelet2MapMeta >	available_maps_
std::string	map_filepath_
std::string	map_frame_id_ = "map"
std::string	map_contents_
double	origin_lat_
double	origin_lon_
bool	origin_lat_set_ = false
bool	origin_lon_set_ = false
std::shared_ptr< tf2_ros::StaticTransformBroadcaster >	tf_static_broadcaster_
rclcpp::Subscription< sensor_msgs::msg::NavSatFix >::SharedPtr	navsat_subscription_
double	current_latitude_ = std::numeric_limits<double>::quiet_NaN()
double	current_longitude_ = std::numeric_limits<double>::quiet_NaN()
bool	gps_fix_received_ = false

Detailed Description

Definition at line 40 of file lanelet2_map_server.hpp.

Constructor & Destructor Documentation

Lanelet2MapServer()

Lanelet2MapServer::Lanelet2MapServer

(

)

Creates the Lanelet2 map server node and declares its parameters.

Definition at line 16 of file lanelet2_map_server.cpp.

                                     : Node("lanelet2_map_server") {
  // General parameters
  this->declareAndLoadParameter("map_frame_id", map_frame_id_, "Frame ID of Lanelet2 map", true, false, false);
  // Automatic map selection parameters
  this->declareAndLoadParameter("use_automatic_map_selection", use_automatic_map_selection_, "Automatic map selection", false,
                                false, true);
  this->declareAndLoadParameter("map_directory", map_directory_, "Directory containing Lanelet2 maps", true, false, false);
  // Map-Server parameters (are not reconfigurable and not required if automatic map selection is enabled)
  this->declareAndLoadParameter("map_filepath", map_filepath_, "Path to Lanelet2 map", !use_automatic_map_selection_,
                                !use_automatic_map_selection_, false);
  origin_lat_set_ = this->declareAndLoadParameter("origin_lat", origin_lat_, "Latitude of origin of Lanelet2 map",
                                                  !use_automatic_map_selection_, false, false);
  origin_lon_set_ = this->declareAndLoadParameter("origin_lon", origin_lon_, "Longitude of origin of Lanelet2 map",
                                                  !use_automatic_map_selection_, false, false);
  // Map-Contents is never required and never reconfigurable
  this->declareAndLoadParameter("map_contents", map_contents_, "Contents of Lanelet2 map", false, false, false);
 
  this->setup();
}

Member Function Documentation

automaticMapUpdateTimerCallback()

void Lanelet2MapServer::automaticMapUpdateTimerCallback ( )

private

Periodically reevaluates the best map when automatic selection is enabled.

Definition at line 389 of file lanelet2_map_server.cpp.

                                                        {
  if (!gps_fix_received_) {
    RCLCPP_WARN(this->get_logger(), "No initial GPS fix received, unable to select map");
    unsetMapParameters();
    return;
  }
 
  const Lanelet2MapMeta* selected_map = nullptr;
  for (const auto& map_meta : available_maps_) {
    // check for invalid map
    if (map_meta.diagonal_length <= 0.0) {
      continue;
    }
    // check for maps outside current GPS location
    if (current_latitude_ < map_meta.min_lat || current_latitude_ > map_meta.max_lat || current_longitude_ < map_meta.min_lon ||
        current_longitude_ > map_meta.max_lon) {
      continue;
    }
    if (selected_map == nullptr || map_meta.diagonal_length < selected_map->diagonal_length) {
      if (map_sanity_check(map_meta.map_path, map_meta.min_lat, map_meta.min_lon)) {
        RCLCPP_DEBUG(this->get_logger(), "Map sanity check passed for '%s'", map_meta.map_path.c_str());
        selected_map = &map_meta;
      } else {
        RCLCPP_WARN(this->get_logger(), "Map sanity check failed for '%s'", map_meta.map_path.c_str());
        continue;
      }
    }
  }
 
  if (selected_map == nullptr) {
    RCLCPP_WARN(this->get_logger(), "No suitable map found for current GPS location (lat=%.9f, lon=%.9f)", current_latitude_,
                current_longitude_);
    unsetMapParameters();
    return;
  }
 
  if (map_filepath_ == selected_map->map_path) {
    RCLCPP_DEBUG(this->get_logger(), "Currently loaded map '%s' is still valid", map_filepath_.c_str());
    return;
  }
 
  map_filepath_ = selected_map->map_path;
  origin_lat_ = selected_map->min_lat;
  origin_lon_ = selected_map->min_lon;
  origin_lat_set_ = false;
  origin_lon_set_ = false;
  this->loadMapContents();
  this->updateMapParameters();
  this->pub_tf();
}

declareAndLoadParameter()

template<typename T >

bool Lanelet2MapServer::declareAndLoadParameter ( const std::string & name, T & param, const std::string & description, const bool add_to_auto_reconfigurable_params = true, const bool is_required = false, const bool read_only = false, const std::optional< double > & from_value = std::nullopt, const std::optional< double > & to_value = std::nullopt, const std::optional< double > & step_value = std::nullopt, const std::string & additional_constraints = "" )

private

Declares and loads a ROS parameter.

Parameters

name	name
param	parameter variable to load into
description	description
add_to_auto_reconfigurable_params	enable reconfiguration of parameter
is_required	whether failure to load parameter will stop node
read_only	set parameter to read-only
from_value	parameter range minimum
to_value	parameter range maximum
step_value	parameter range step
additional_constraints	additional constraints description

Returns

true if the parameter was already initialized from an external source

Definition at line 37 of file lanelet2_map_server.cpp.

                                                                                         {
  rcl_interfaces::msg::ParameterDescriptor param_desc;
  param_desc.description = description;
  param_desc.additional_constraints = additional_constraints;
  param_desc.read_only = read_only;
 
  auto type = rclcpp::ParameterValue(param).get_type();
 
  if (from_value.has_value() && to_value.has_value()) {
    if constexpr (std::is_integral_v<T>) {
      rcl_interfaces::msg::IntegerRange range;
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
      if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
      param_desc.integer_range = {range};
    } else if constexpr (std::is_floating_point_v<T>) {
      rcl_interfaces::msg::FloatingPointRange range;
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
      if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
      param_desc.floating_point_range = {range};
    } else {
      RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str());
    }
  }
 
  this->declare_parameter(name, type, param_desc);
  bool initial_set = false;
 
  try {
    param = this->get_parameter(name).get_value<T>();
    std::stringstream ss;
    ss << "Loaded parameter '" << name << "': ";
    if constexpr (is_vector_v<T>) {
      ss << "[";
      for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
      ss << "]";
    } else {
      ss << param;
    }
    RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
    initial_set = true;
  } catch (rclcpp::exceptions::ParameterUninitializedException&) {
    if (is_required) {
      RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting");
      exit(EXIT_FAILURE);
    } else {
      std::stringstream ss;
      ss << "Missing parameter '" << name << "', using default value: ";
      if constexpr (is_vector_v<T>) {
        ss << "[";
        for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
        ss << "]";
      } else {
        ss << param;
      }
      RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
      this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
    }
  }
 
  if (add_to_auto_reconfigurable_params) {
    std::function<void(const rclcpp::Parameter&)> setter = [&param](const rclcpp::Parameter& p) { param = p.get_value<T>(); };
    auto_reconfigurable_params_.push_back(std::make_tuple(name, setter));
  }
 
  return initial_set;
}

derive_map_meta()

void Lanelet2MapServer::derive_map_meta ( Lanelet2MapMeta & map_meta ) const

private

Fills the derived metadata fields for a discovered map.

Parameters

map_meta

metadata entry whose geographic bounds and size are updated

Definition at line 328 of file lanelet2_map_server.cpp.

                                                                       {
  map_meta.diagonal_length = -1.0;
  pugi::xml_document document;
  const pugi::xml_parse_result result = document.load_file(map_meta.map_path.c_str());
  if (!result) {
    RCLCPP_ERROR_STREAM(get_logger(), "Unable to parse map '" << map_meta.map_path << "': " << result.description());
    return;
  }
 
  pugi::xml_node root = document.child("osm");
  if (!root) {
    root = document.document_element();
  }
  if (!root) {
    RCLCPP_WARN_STREAM(get_logger(), "No root element found when deriving meta for '" << map_meta.map_path << "'");
    return;
  }
 
  const double kDoubleMax = std::numeric_limits<double>::max();
  const double kDoubleLowest = std::numeric_limits<double>::lowest();
  double min_lat = kDoubleMax;
  double min_lon = kDoubleMax;
  double max_lat = kDoubleLowest;
  double max_lon = kDoubleLowest;
  bool found_node = false;
 
  for (pugi::xml_node node : root.children("node")) {
    const double lat = node.attribute("lat").as_double(std::numeric_limits<double>::quiet_NaN());
    const double lon = node.attribute("lon").as_double(std::numeric_limits<double>::quiet_NaN());
    if (std::isnan(lat) || std::isnan(lon)) {
      continue;
    }
 
    min_lat = std::min(min_lat, lat);
    max_lat = std::max(max_lat, lat);
    min_lon = std::min(min_lon, lon);
    max_lon = std::max(max_lon, lon);
    found_node = true;
  }
 
  if (!found_node) {
    RCLCPP_WARN_STREAM(get_logger(), "No node elements found while deriving bounds for '" << map_meta.map_path << "'");
    return;
  }
 
  map_meta.min_lat = min_lat;
  map_meta.max_lat = max_lat;
  map_meta.min_lon = min_lon;
  map_meta.max_lon = max_lon;
 
  double distance = 0.0;
  GeographicLib::Geodesic::WGS84().Inverse(min_lat, min_lon, max_lat, max_lon, distance);
  map_meta.diagonal_length = distance;
}

derive_utm_zone()

void Lanelet2MapServer::derive_utm_zone ( const double latitude, const double longitude, int & zone, bool & northp )

staticprivate

Derives the UTM zone and hemisphere from a geographic position.

Parameters

latitude	latitude in degrees
longitude	longitude in degrees
zone	output UTM zone number
northp	output hemisphere flag, true for northern hemisphere

Definition at line 479 of file lanelet2_map_server.cpp.

                                                                                                              {
  northp = latitude >= 0.0;
  zone = static_cast<int>(std::floor((longitude + 180.0) / 6.0)) + 1;
}

deriveOriginFromMap()

bool Lanelet2MapServer::deriveOriginFromMap ( const std::string & map_filepath, double & origin_lat, double & origin_lon ) const

private

Derives a south-west origin latitude and longitude from a Lanelet2 map file.

Parameters

map_filepath	path to the map file
origin_lat	output latitude of the derived south-west origin
origin_lon	output longitude of the derived south-west origin

Returns

true if the origin could be derived from the map bounds

Definition at line 283 of file lanelet2_map_server.cpp.

                                                                                                                       {
  if (map_filepath.empty()) {
    RCLCPP_ERROR(this->get_logger(), "No map filepath provided, cannot derive origin from map.");
    return false;
  }
 
  Lanelet2MapMeta map_meta;
  map_meta.map_path = map_filepath;
  derive_map_meta(map_meta);
  if (map_meta.diagonal_length <= 0.0) {
    return false;
  }
 
  origin_lat = map_meta.min_lat;
  origin_lon = map_meta.min_lon;
  return true;
}

find_available_maps()

void Lanelet2MapServer::find_available_maps ( const std::string & directory, std::vector< Lanelet2MapMeta > & maps ) const

private

Recursively scans a directory for available Lanelet2 map files.

Parameters

directory	root directory to search
maps	output vector that receives discovered map metadata

Definition at line 301 of file lanelet2_map_server.cpp.

                                                                                                              {
  maps.clear();
  try {
    for (const auto& entry : std::filesystem::recursive_directory_iterator(directory)) {
      if (!entry.is_regular_file()) {
        continue;
      }
      if (entry.path().extension() == ".osm") {
        Lanelet2MapMeta map_meta;
        map_meta.map_path = entry.path().string();
        maps.emplace_back(map_meta);
      }
    }
  } catch (const std::filesystem::filesystem_error& e) {
    RCLCPP_ERROR_STREAM(get_logger(), "Failed to scan '" << directory << "': " << e.what());
  }
  for (auto& map_meta : maps) {
    derive_map_meta(map_meta);
  }
  RCLCPP_INFO(this->get_logger(), "Discovered %zu Lanelet2 maps in '%s'", maps.size(), directory.c_str());
  for (const auto& map_meta : available_maps_) {
    RCLCPP_INFO(this->get_logger(), "Map '%s': min_lat=%.9f, min_lon=%.9f, max_lat=%.9f, max_lon=%.9f, diagonal_length=%.3f",
                map_meta.map_path.c_str(), map_meta.min_lat, map_meta.min_lon, map_meta.max_lat, map_meta.max_lon,
                map_meta.diagonal_length);
  }
}

loadMapContents()

void Lanelet2MapServer::loadMapContents ( )

private

Loads the configured map file and caches its serialized contents.

Definition at line 440 of file lanelet2_map_server.cpp.

                                        {
  if (map_filepath_.empty()) {
    map_contents_ = "";
    return;
  }
  std::ifstream file(map_filepath_);
  map_contents_ = std::string((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
  RCLCPP_INFO(this->get_logger(), "Loaded map contents from '%s' to parameter 'map_contents'", map_filepath_.c_str());
}

map_sanity_check()

bool Lanelet2MapServer::map_sanity_check ( std::string map_filepath, double origin_lat, double origin_lon ) const

private

Tries to load a map with the provided origin to verify that it is usable.

Parameters

map_filepath	path to the map file
origin_lat	latitude used for projection
origin_lon	longitude used for projection

Returns

true if the map passes the sanity check

Definition at line 467 of file lanelet2_map_server.cpp.

                                                                                                           {
  lanelet::projection::UtmProjector proj(lanelet::Origin({origin_lat, origin_lon}));
  try {
    lanelet::LaneletMapPtr map = lanelet::load(map_filepath, proj);
  } catch (const lanelet::IOError& e) {
    RCLCPP_ERROR_STREAM(this->get_logger(), "Could not load map '" << map_filepath << "': " << e.what());
    return false;
  }
 
  return true;
}

navSatFixCallback()

void Lanelet2MapServer::navSatFixCallback ( const sensor_msgs::msg::NavSatFix::SharedPtr msg )

private

Stores the latest GNSS fix for automatic map selection.

Parameters

msg	incoming GPS fix message

Definition at line 383 of file lanelet2_map_server.cpp.

                                                                                    {
  current_latitude_ = msg->latitude;
  current_longitude_ = msg->longitude;
  gps_fix_received_ = true;
}

parametersCallback()

rcl_interfaces::msg::SetParametersResult Lanelet2MapServer::parametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

private

Handles reconfiguration when a parameter value is changed.

Parameters

parameters

parameters

Returns

parameter change result

Definition at line 113 of file lanelet2_map_server.cpp.

                                                                                                                       {
  std::string previous_map_filepath = map_filepath_;
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = false;
  double previous_origin_lat = origin_lat_;
  double previous_origin_lon = origin_lon_;
  bool relevant_change = false;
 
  bool map_filepath_changed = false;
  bool origin_lat_changed = false;
  bool origin_lon_changed = false;
 
  RCLCPP_INFO(this->get_logger(), "Received request to change %zu parameter(s)", parameters.size());
  for (const auto& p : parameters) {
    if (p.get_name() == "map_contents") {
      RCLCPP_INFO(this->get_logger(), "Incoming param: map_contents = <omitted, size=%zu>", p.get_value<std::string>().size());
    } else {
      RCLCPP_INFO(this->get_logger(), "Incoming param: %s = %s", p.get_name().c_str(), p.value_to_string().c_str());
    }
  }
 
  for (const auto& param : parameters) {
    for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) {
      if (param.get_name() == std::get<0>(auto_reconfigurable_param)) {
        std::get<1>(auto_reconfigurable_param)(param);
        RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s' to: %s", param.get_name().c_str(),
                    param.value_to_string().c_str());
        break;
      }
    }
    // handle special cases
    if (param.get_name() == "map_filepath") {
      if (param.get_value<std::string>() != map_filepath_) {
        result.reason = "Parameter 'map_filepath' is not reconfigurable when automatic map selection is enabled.";
        return result;
      }
      map_filepath_changed = previous_map_filepath != map_filepath_;
      relevant_change = relevant_change || map_filepath_changed;
    }
    if (param.get_name() == "origin_lat") {
      if (param.get_value<double>() != origin_lat_) {
        result.reason = "Parameter 'origin_lat' is not reconfigurable when automatic map selection is enabled.";
        return result;
      }
      origin_lat_changed = previous_origin_lat != origin_lat_;
      relevant_change = relevant_change || origin_lat_changed;
 
      if (origin_lat_changed) origin_lat_set_ = true;
    }
    if (param.get_name() == "origin_lon") {
      if (param.get_value<double>() != origin_lon_) {
        result.reason = "Parameter 'origin_lon' is not reconfigurable when automatic map selection is enabled.";
        return result;
      }
      origin_lon_changed = previous_origin_lon != origin_lon_;
      relevant_change = relevant_change || origin_lon_changed;
 
      if (origin_lon_changed) origin_lon_set_ = true;
    }
    if (param.get_name() == "map_contents" && param.get_value<std::string>() != map_contents_) {
      result.reason = "Parameter 'map_contents' is not reconfigurable.";
      return result;
    }
    if (param.get_name() == "map_directory" && use_automatic_map_selection_) {
      find_available_maps(map_directory_, available_maps_);
      if (available_maps_.empty()) {
        RCLCPP_ERROR(this->get_logger(), "No Lanelet2 maps found in '%s'", map_directory_.c_str());
        // reload parameters in timer callback since parameters cannot be updated in this callback
        one_shot_timer_ = this->create_wall_timer(std::chrono::milliseconds(1), [this]() {
          this->one_shot_timer_->cancel();
          unsetMapParameters();
        });
      }
    }
  }
 
  if (!use_automatic_map_selection_ && relevant_change) {
    // set default origin (bottom left) if not set before
    if (!origin_lat_set_ && !origin_lon_set_) {
      if (!deriveOriginFromMap(map_filepath_, origin_lat_, origin_lon_)) {
        result.reason = "Could not derive lower-left origin from map in manual mode.";
        result.successful = false;
        return result;
      }
 
      RCLCPP_INFO(this->get_logger(), "Derived manual origin from lower-left map corner for '%s' (lat=%.9f, lon=%.9f)",
                  map_filepath_.c_str(), origin_lat_, origin_lon_);
    }
 
    // load map if not unbalanced origin parameters are provided (both or none)
    if (origin_lat_set_ == origin_lon_set_) {
      if (this->map_sanity_check(map_filepath_, origin_lat_, origin_lon_)) {
        // reload parameters in timer callback since parameters cannot be updated in this callback
        one_shot_timer_ = this->create_wall_timer(std::chrono::milliseconds(1), [this]() {
          this->one_shot_timer_->cancel();
 
          this->loadMapContents();
          this->updateMapParameters();
          this->pub_tf();
        });
      } else {
        this->updateMapParameters();
        RCLCPP_ERROR(this->get_logger(), "Map sanity check failed for map '%s' with origin (lat=%.9f, lon=%.9f)",
                     map_filepath_.c_str(), origin_lat_, origin_lon_);
        result.reason = "Map sanity check failed in manual mode.";
        result.successful = false;
        return result;
      }
    } else {
      this->updateMapParameters();
      RCLCPP_WARN(this->get_logger(),
                  "Waiting for origin readiness: if origin is explictly set, both parameters must be provided.");
      result.reason = "Waiting for origin readiness: if origin is explictly set, both parameters must be provided.";
      result.successful = true;
      return result;
    }
  }
  result.successful = true;
  return result;
}

pub_tf()

void Lanelet2MapServer::pub_tf ( ) const

private

Publishes the static transform from the derived UTM frame to the map frame.

Definition at line 484 of file lanelet2_map_server.cpp.

                                     {
  if (map_filepath_.empty()) {
    return;
  }
  geometry_msgs::msg::TransformStamped t;
 
  // Create Projector without offset
  lanelet::projection::UtmProjector proj_utm(lanelet::Origin({origin_lat_, origin_lon_}), false);
  lanelet::BasicPoint3d origin_utm = proj_utm.forward(lanelet::GPSPoint({origin_lat_, origin_lon_, 0.0}));
 
  t.transform.translation.x = origin_utm.x();
  t.transform.translation.y = origin_utm.y();
  t.transform.translation.z = origin_utm.z();
 
  t.transform.rotation.x = 0;
  t.transform.rotation.y = 0;
  t.transform.rotation.z = 0;
  t.transform.rotation.w = 1;
 
  bool northp = false;
  int zone = 0;
  derive_utm_zone(origin_lat_, origin_lon_, zone, northp);
 
  std::string hemisphere;
  if (northp) {
    hemisphere = "N";
  } else {
    hemisphere = "S";
  }
 
  t.header.frame_id = "utm_" + std::to_string(zone) + hemisphere;
  t.child_frame_id = map_frame_id_;
  t.header.stamp = now();
 
  // Send the transformation
  tf_static_broadcaster_->sendTransform(t);
  RCLCPP_INFO(get_logger(), "Broadcast static transform from '%s' to '%s'", t.header.frame_id.c_str(), t.child_frame_id.c_str());
}

setup()

void Lanelet2MapServer::setup ( )

private

Sets up timers, callbacks, publishers, and initial map loading.

Definition at line 234 of file lanelet2_map_server.cpp.

                              {
  // callback for dynamic parameter configuration
  parameters_callback_ =
      this->add_on_set_parameters_callback(std::bind(&Lanelet2MapServer::parametersCallback, this, std::placeholders::_1));
 
  tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
 
  if (use_automatic_map_selection_) {
    find_available_maps(map_directory_, available_maps_);
    if (available_maps_.empty()) {
      RCLCPP_ERROR(this->get_logger(), "No Lanelet2 maps found in '%s'", map_directory_.c_str());
      unsetMapParameters();
    }
    // Create NavSatFix subscription and automatic map update timer
    navsat_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
        "~/gps/fix", 10, std::bind(&Lanelet2MapServer::navSatFixCallback, this, std::placeholders::_1));
    automatic_map_timer_ =
        this->create_wall_timer(std::chrono::seconds(1), std::bind(&Lanelet2MapServer::automaticMapUpdateTimerCallback, this));
  } else {
    if (!origin_lat_set_ && !origin_lon_set_) {
      if (!deriveOriginFromMap(map_filepath_, origin_lat_, origin_lon_)) {
        RCLCPP_ERROR(this->get_logger(), "Could not derive lower-left origin from map '%s' in manual mode.",
                     map_filepath_.c_str());
        unsetMapParameters();
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Derived manual origin from lower-left map corner for '%s' (lat=%.9f, lon=%.9f)",
                  map_filepath_.c_str(), origin_lat_, origin_lon_);
    } else if (origin_lat_set_ != origin_lon_set_) {
      RCLCPP_ERROR(this->get_logger(),
                   "Manual mode requires both 'origin_lat' and 'origin_lon' when either is provided. Received origin_lat set=%s, "
                   "origin_lon set=%s.",
                   origin_lat_set_ ? "true" : "false", origin_lon_set_ ? "true" : "false");
      unsetMapParameters();
      return;
    }
 
    if (this->map_sanity_check(map_filepath_, origin_lat_, origin_lon_)) {
      this->loadMapContents();
      this->updateMapParameters();
      this->pub_tf();
    } else {
      RCLCPP_ERROR(this->get_logger(), "Map sanity check failed for map '%s' with origin (lat=%.9f, lon=%.9f)",
                   map_filepath_.c_str(), origin_lat_, origin_lon_);
      unsetMapParameters();
    }
  }
}

unsetMapParameters()

void Lanelet2MapServer::unsetMapParameters ( )

private

Clears map-specific parameters after an invalid or unloaded map state.

Definition at line 457 of file lanelet2_map_server.cpp.

                                           {
  map_filepath_ = "";
  map_contents_ = "";
  origin_lat_ = 0.0;
  origin_lon_ = 0.0;
  origin_lat_set_ = false;
  origin_lon_set_ = false;
  updateMapParameters();
}

updateMapParameters()

void Lanelet2MapServer::updateMapParameters ( )

private

Writes the current map state into this node's exported ROS parameters.

Definition at line 450 of file lanelet2_map_server.cpp.

                                            {
  this->set_parameter(rclcpp::Parameter("map_filepath", map_filepath_));
  this->set_parameter(rclcpp::Parameter("map_contents", map_contents_));
  this->set_parameter(rclcpp::Parameter("origin_lat", origin_lat_));
  this->set_parameter(rclcpp::Parameter("origin_lon", origin_lon_));
}

Member Data Documentation

auto_reconfigurable_params_

std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)> > > Lanelet2MapServer::auto_reconfigurable_params_

private

Auto-reconfigurable parameters for dynamic reconfiguration.

Definition at line 168 of file lanelet2_map_server.hpp.

automatic_map_timer_

rclcpp::TimerBase::SharedPtr Lanelet2MapServer::automatic_map_timer_

private

Definition at line 176 of file lanelet2_map_server.hpp.

available_maps_

std::vector<Lanelet2MapMeta> Lanelet2MapServer::available_maps_

private

Definition at line 181 of file lanelet2_map_server.hpp.

current_latitude_

double Lanelet2MapServer::current_latitude_ = std::numeric_limits<double>::quiet_NaN()

private

Definition at line 192 of file lanelet2_map_server.hpp.

current_longitude_

double Lanelet2MapServer::current_longitude_ = std::numeric_limits<double>::quiet_NaN()

private

Definition at line 193 of file lanelet2_map_server.hpp.

gps_fix_received_

bool Lanelet2MapServer::gps_fix_received_ = false

private

Definition at line 194 of file lanelet2_map_server.hpp.

map_contents_

std::string Lanelet2MapServer::map_contents_

private

Definition at line 184 of file lanelet2_map_server.hpp.

map_directory_

std::string Lanelet2MapServer::map_directory_ = "/data/maps/default-maps"

private

Definition at line 180 of file lanelet2_map_server.hpp.

map_filepath_

std::string Lanelet2MapServer::map_filepath_

private

Definition at line 182 of file lanelet2_map_server.hpp.

map_frame_id_

std::string Lanelet2MapServer::map_frame_id_ = "map"

private

Definition at line 183 of file lanelet2_map_server.hpp.

navsat_subscription_

rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr Lanelet2MapServer::navsat_subscription_

private

Definition at line 191 of file lanelet2_map_server.hpp.

one_shot_timer_

rclcpp::TimerBase::SharedPtr Lanelet2MapServer::one_shot_timer_

private

Definition at line 175 of file lanelet2_map_server.hpp.

origin_lat_

double Lanelet2MapServer::origin_lat_

private

Definition at line 185 of file lanelet2_map_server.hpp.

origin_lat_set_

bool Lanelet2MapServer::origin_lat_set_ = false

private

Definition at line 187 of file lanelet2_map_server.hpp.

origin_lon_

double Lanelet2MapServer::origin_lon_

private

Definition at line 186 of file lanelet2_map_server.hpp.

origin_lon_set_

bool Lanelet2MapServer::origin_lon_set_ = false

private

Definition at line 188 of file lanelet2_map_server.hpp.

parameters_callback_

OnSetParametersCallbackHandle::SharedPtr Lanelet2MapServer::parameters_callback_

private

Callback handle for dynamic parameter reconfiguration.

Definition at line 173 of file lanelet2_map_server.hpp.

tf_static_broadcaster_

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> Lanelet2MapServer::tf_static_broadcaster_

private

Definition at line 190 of file lanelet2_map_server.hpp.

use_automatic_map_selection_

bool Lanelet2MapServer::use_automatic_map_selection_ = false

private

Definition at line 178 of file lanelet2_map_server.hpp.

---

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

• lanelet2_map_server/include/lanelet2_map_server/lanelet2_map_server.hpp
• lanelet2_map_server/src/lanelet2_map_server.cpp