lanelet2_lichtblick_display::Lanelet2LichtblickDisplay Class Reference

Lanelet2LichtblickDisplay class. More...

#include <lanelet2_lichtblick_display.hpp>

Inheritance diagram for lanelet2_lichtblick_display::Lanelet2LichtblickDisplay:

Public Member Functions
	Lanelet2LichtblickDisplay ()
	Creates the Lichtblick visualization node and declares its rendering parameters.

Private Member Functions
template<typename T >
void	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	setup ()
	Sets up subscribers, publishers, etc. to configure the node.
void	checkMapStatus ()
	Check for a new or changed Lanelet2 map and publish markers if present.
void	hexToRgb (const std::string &hex, float &r, float &g, float &b)
	Convert a hex color string into normalized RGB float components.
void	publishMarker (const lanelet::LaneletMapConstPtr &lanelet_map)
	Converts a Lanelet2 map into a MarkerArray and publishes it.
void	clearAllMarkers ()
	Publishes a DELETEALL action to clear all markers from the topic.
void	addLineStripMarker (visualization_msgs::msg::MarkerArray &marker_array_msg, int &current_marker_id, const std::string &ns, double line_width, const std::string &color_hex, double opacity, const std_msgs::msg::Header &header, const std::vector< geometry_msgs::msg::Point > &points)
	Creates and adds a LINE_STRIP marker to a marker array.

Static Private Member Functions
static geometry_msgs::msg::Point	toRos (const Eigen::Vector3d &point)
	Converts an Eigen::Vector3d into a ROS geometry_msgs::msg::Point.
static void	addMeshMarker (visualization_msgs::msg::MarkerArray &marker_array_msg, int &current_marker_id, const std::string &ns, const std::string &mesh_resource, const geometry_msgs::msg::Point &position, double scale, double z_offset, double opacity, const std_msgs::msg::Header &header, double yaw_ref_line, const geometry_msgs::msg::Point &middle_point)
	Creates and adds a MESH_RESOURCE marker for regulatory elements.
static std::optional< std::array< geometry_msgs::msg::Point, 2 > >	regulatoryElementReferenceLine (const std::shared_ptr< const lanelet::RegulatoryElement > &regulatory_element)
	Extracts the reference/effect line of a regulatory element.
static std::vector< geometry_msgs::msg::Point >	regulatoryElementPositions (const std::shared_ptr< const lanelet::RegulatoryElement > &regulatory_element)
	Extracts the sign/signal positions of a regulatory element.

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::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	marker_array_publisher_
	Publisher Marker Array.
rclcpp::TimerBase::SharedPtr	timer_
std::atomic< bool >	need_republish_ {false}
std::shared_ptr< Lanelet2MapInterface >	lanelet2_map_interface_
lanelet::LaneletMapConstPtr	last_map_ptr_
std::string	map_frame_id
rclcpp::Time	current_timestamp
double	left_bound_line_width_ = 0.1
std::string	left_bound_color_hex_ = "#0000FF"
double	left_bound_line_opacity_ = 0.5
double	right_bound_line_width_ = 0.1
std::string	right_bound_color_hex_ = "#FF0000"
double	right_bound_line_opacity_ = 0.5
double	centerline_line_width_ = 0.08
std::string	centerline_color_hex_ = "#008000"
double	centerline_line_opacity_ = 0.4
double	lanelet_text_scale_ = 0.5
std::string	lanelet_text_color_hex_ = "#000000"
double	lanelet_text_opacity_ = 1.0
double	reference_line_width_ = 0.2
std::string	reference_line_color_hex_ = "#FFFF00"
double	reference_line_opacity_ = 0.5
std::string	traffic_light_mesh_resource_
double	traffic_light_scale_ = 1.0
double	traffic_light_z_offset_ = 1.3
double	traffic_light_opacity_ = 1.0
std::string	yield_sign_mesh_resource_
double	yield_sign_scale_ = 1.0
double	yield_sign_z_offset_ = 0.0
double	yield_sign_opacity_ = 1.0

Detailed Description

Lanelet2LichtblickDisplay class.

Definition at line 34 of file lanelet2_lichtblick_display.hpp.

Constructor & Destructor Documentation

Lanelet2LichtblickDisplay()

lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::Lanelet2LichtblickDisplay

(

)

Creates the Lichtblick visualization node and declares its rendering parameters.

Definition at line 8 of file lanelet2_lichtblick_display.cpp.

                                                     : Node("lanelet2_lichtblick_display") {
  this->declareAndLoadParameter("left_bound_line_width", left_bound_line_width_, "Width of the left boundary lines", true, false,
                                false);
  this->declareAndLoadParameter("left_bound_color_hex", left_bound_color_hex_, "Color of the left boundary lines", true, false,
                                false);
  this->declareAndLoadParameter("left_bound_line_opacity", left_bound_line_opacity_, "Opacity of the left boundary lines", true,
                                false, false);
 
  this->declareAndLoadParameter("right_bound_line_width", right_bound_line_width_, "Width of the right boundary lines", true,
                                false, false);
  this->declareAndLoadParameter("right_bound_color_hex", right_bound_color_hex_, "Color of the right boundary lines", true, false,
                                false);
  this->declareAndLoadParameter("right_bound_line_opacity", right_bound_line_opacity_, "Opacity of the right boundary lines",
                                true, false, false);
 
  this->declareAndLoadParameter("centerline_line_width", centerline_line_width_, "Width of the centerlines", true, false, false);
  this->declareAndLoadParameter("centerline_color_hex", centerline_color_hex_, "Color of the centerlines", true, false, false);
  this->declareAndLoadParameter("centerline_line_opacity", centerline_line_opacity_, "Opacity of the centerlines", true, false,
                                false);
 
  this->declareAndLoadParameter("lanelet_text_scale", lanelet_text_scale_, "Scale of the lanelet ID text", true, false, false);
  this->declareAndLoadParameter("lanelet_text_color", lanelet_text_color_hex_, "Color of the lanelet ID text", true, false,
                                false);
  this->declareAndLoadParameter("lanelet_text_opacity", lanelet_text_opacity_, "Opacity of the lanelet ID text", true, false,
                                false);
 
  this->declareAndLoadParameter("reference_line_width", reference_line_width_, "Width of the reference lines", true, false,
                                false);
  this->declareAndLoadParameter("reference_line_color_hex", reference_line_color_hex_, "Color of the reference lines", true,
                                false, false);
  this->declareAndLoadParameter("reference_line_opacity", reference_line_opacity_, "Opacity of the reference lines", true, false,
                                false);
 
  this->declareAndLoadParameter("traffic_light_mesh_resource", traffic_light_mesh_resource_,
                                "Link to the traffic light model to use", true, false, false);
  this->declareAndLoadParameter("traffic_light_scale", traffic_light_scale_, "Scale of the traffic lights models", true, false,
                                false);
  this->declareAndLoadParameter("traffic_light_z_offset", traffic_light_z_offset_,
                                "Offset in z-direction of the traffic lights models (depends on model and scale)", true, false,
                                false);
  this->declareAndLoadParameter("traffic_light_opacity", traffic_light_opacity_, "Opacity of the traffic lights", true, false,
                                false);
 
  this->declareAndLoadParameter("yield_sign_mesh_resource", yield_sign_mesh_resource_, "Link to the yield sign model to use",
                                true, false, false);
  this->declareAndLoadParameter("yield_sign_scale", yield_sign_scale_, "Scale of the yield sign models", true, false, false);
  this->declareAndLoadParameter("yield_sign_z_offset", yield_sign_z_offset_,
                                "Offset in z-direction of the yield sign models (depends on model and scale)", true, false,
                                false);
  this->declareAndLoadParameter("yield_sign_opacity", yield_sign_opacity_, "Opacity of the yield signs", true, false, false);
 
  this->setup();
}

Member Function Documentation

addLineStripMarker()

void lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::addLineStripMarker ( visualization_msgs::msg::MarkerArray & marker_array_msg, int & current_marker_id, const std::string & ns, double line_width, const std::string & color_hex, double opacity, const std_msgs::msg::Header & header, const std::vector< geometry_msgs::msg::Point > & points )

private

Creates and adds a LINE_STRIP marker to a marker array.

• Encapsulates the repetitive logic for visualizing line segments. It simplifies the main publishing loop by handling all marker properties based on the provided parameters.

• Parameters

  marker_array_msg	The marker array message to which the new marker will be added.
  current_marker_id	A reference to the marker ID counter, which will be incremented.
  ns	The namespace for the marker, used for grouping in the visualization tool.
  line_width	The width of the line in meters.
  color_hex	The hexadecimal color string.
  opacity	The opacity of the line.
  header	The ROS header containing the frame ID and timestamp.
  points	A vector of geometry_msgs::msg::Point objects defining the line segments.

Definition at line 220 of file lanelet2_lichtblick_display.cpp.

                                                                                                   {
  if (points.empty()) {
    return;
  }
 
  visualization_msgs::msg::Marker marker;
  marker.header = header;
  marker.ns = ns;
  marker.id = current_marker_id++;                                   // give every marker a unique ID
  marker.lifetime = rclcpp::Duration(std::chrono::milliseconds(0));  // indefinite since QoS is set to transient local
  marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.scale.x = line_width;
 
  float r = 0.0F;
  float g = 0.0F;
  float b = 0.0F;
  hexToRgb(color_hex, r, g, b);
  marker.color.r = r;
  marker.color.g = g;
  marker.color.b = b;
  marker.color.a = static_cast<float>(opacity);
  marker.points = points;
 
  marker_array_msg.markers.push_back(marker);
}

addMeshMarker()

void lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::addMeshMarker ( visualization_msgs::msg::MarkerArray & marker_array_msg, int & current_marker_id, const std::string & ns, const std::string & mesh_resource, const geometry_msgs::msg::Point & position, double scale, double z_offset, double opacity, const std_msgs::msg::Header & header, double yaw_ref_line, const geometry_msgs::msg::Point & middle_point )

staticprivate

Creates and adds a MESH_RESOURCE marker for regulatory elements.

• Streamlines the process of visualizing 3D models of traffic signs. It handles the common logic for setting marker properties, including calculating the orientation to be orthogonal to a reference line.

• Parameters

  marker_array_msg	The marker array message to which the new marker will be added.
  current_marker_id	A reference to the marker ID counter, which will be incremented.
  ns	The namespace for the marker.
  mesh_resource	The URI of the 3D mesh model.
  position	The base position of the mesh marker.
  scale	The uniform scale factor for the mesh.
  z_offset	A vertical offset to correctly position the mesh above the ground.
  opacity	The opacity of the mesh marker.
  header	The ROS header.
  yaw_ref_line	The yaw of the reference line, used to calculate orthogonal orientation.
  middle_point	The middle point of the reference line, used to determine orientation.

Definition at line 254 of file lanelet2_lichtblick_display.cpp.

                                                                                         {
  visualization_msgs::msg::Marker marker;
  marker.header = header;
  marker.ns = ns;
  marker.id = current_marker_id++;                                   // give every marker a unique ID
  marker.lifetime = rclcpp::Duration(std::chrono::milliseconds(0));  // indefinite since QoS is set to transient local
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
  marker.mesh_resource = mesh_resource;
  // uniform scale
  marker.scale.x = scale;
  marker.scale.y = scale;
  marker.scale.z = scale;
 
  // set the position of the mesh marker (depends on scale and model)
  geometry_msgs::msg::Point adjusted_position = position;
  adjusted_position.z = z_offset;
  marker.pose.position = adjusted_position;
 
  // calculate orientation orthogonal to the reference line
  // 1. need vector from traffic sign to the reference line
  double dx_to_ref = middle_point.x - position.x;
  double dy_to_ref = middle_point.y - position.y;
  double yaw_to_ref = std::atan2(dy_to_ref, dx_to_ref);
  // 2. two possible perpendicular orientations to the reference line
  double cand1 = yaw_ref_line + M_PI_2;  // +90°
  double cand2 = yaw_ref_line - M_PI_2;  // -90°
  // measure angular difference (normalized) using atan2(sin,cos)
  double d1 = std::abs(std::atan2(std::sin(yaw_to_ref - cand1), std::cos(yaw_to_ref - cand1)));
  double d2 = std::abs(std::atan2(std::sin(yaw_to_ref - cand2), std::cos(yaw_to_ref - cand2)));
  double final_yaw = (d1 < d2) ? cand1 : cand2;
 
  tf2::Quaternion q;
  q.setRPY(0, 0, final_yaw + M_PI_2);
  marker.pose.orientation = tf2::toMsg(q);
 
  marker.mesh_use_embedded_materials = true;
  marker.color.a = static_cast<float>(opacity);
 
  marker_array_msg.markers.push_back(marker);
}

checkMapStatus()

void lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::checkMapStatus ( )

private

Check for a new or changed Lanelet2 map and publish markers if present.

This function queries the Lanelet2MapInterface for the current map pointer and compares it against the internally stored last_map_ptr_. If a new map is detected (pointer change and non-null), it triggers publishMarker(...) to convert the map into visualization markers.

Called periodically by the node's timer.

Definition at line 335 of file lanelet2_lichtblick_display.cpp.

                                               {
  lanelet::LaneletMapConstPtr current_map_ptr = lanelet2_map_interface_->getMapPtr();
 
  // Check if the map has been loaded or if a new map has been received
  if (current_map_ptr && current_map_ptr != last_map_ptr_) {
    RCLCPP_INFO(this->get_logger(), "New Lanelet2 map detected. Publishing markers...");
 
    map_frame_id = lanelet2_map_interface_->map_frame_id_;
    this->clearAllMarkers();
 
    this->publishMarker(current_map_ptr);
    last_map_ptr_ = current_map_ptr;
    need_republish_.store(false);  // cleared after publish
  }
 
  // Check if parameters changed
  if (need_republish_.load() && last_map_ptr_) {
    RCLCPP_INFO(this->get_logger(), "Parameters changed. Republishing markers...");
    this->publishMarker(last_map_ptr_);
    need_republish_.store(false);
  }
}

clearAllMarkers()

void lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::clearAllMarkers ( )

private

Publishes a DELETEALL action to clear all markers from the topic.

• This function creates a single visualization_msgs::msg::Marker with the DELETEALL action and publishes it. This action signals to the visualization toolto remove all markers previously published on the topic, ensuring a clean slate for new visualizations.

Definition at line 208 of file lanelet2_lichtblick_display.cpp.

                                                {
  visualization_msgs::msg::MarkerArray marker_array_msg;
  visualization_msgs::msg::Marker delete_marker;
 
  delete_marker.header.frame_id = map_frame_id;
  delete_marker.header.stamp = this->now();
  delete_marker.action = visualization_msgs::msg::Marker::DELETEALL;
 
  marker_array_msg.markers.push_back(delete_marker);
  marker_array_publisher_->publish(marker_array_msg);
}

declareAndLoadParameter()

template<typename T >

void lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::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

Definition at line 63 of file lanelet2_lichtblick_display.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;
      T step = static_cast<T>(step_value.has_value() ? step_value.value() : 1);
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value())).set__step(step);
      param_desc.integer_range = {range};
    } else if constexpr (std::is_floating_point_v<T>) {
      rcl_interfaces::msg::FloatingPointRange range;
      T step = static_cast<T>(step_value.has_value() ? step_value.value() : 1.0);
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value())).set__step(step);
      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);
 
  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());
  } 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));
  }
}

hexToRgb()

void lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::hexToRgb ( const std::string & hex, float & r, float & g, float & b )

private

Convert a hex color string into normalized RGB float components.

Accepted formats: "#RRGGBB", "0xRRGGBB", or "RRGGBB". If the input is invalid (wrong length or parse error), the function writes black (0,0,0) into r,g,b and logs a warning.

Definition at line 170 of file lanelet2_lichtblick_display.cpp.

                                                                                           {
  std::string clean_hex = hex;
  if (clean_hex.rfind("0x", 0) == 0) {
    clean_hex = clean_hex.substr(2);
  } else if (clean_hex.rfind("#", 0) == 0) {
    clean_hex = clean_hex.substr(1);
  }
  if (clean_hex.length() != 6) {
    RCLCPP_WARN(this->get_logger(), "Invalid hex color string: %s. Defaulting to black.", hex.c_str());
    r = g = b = 0.0F;
    return;
  }
 
  unsigned int r_int = 0U;
  unsigned int g_int = 0U;
  unsigned int b_int = 0U;
 
  std::istringstream rs(clean_hex.substr(0, 2));
  rs >> std::hex >> r_int;
  r = static_cast<float>(r_int) / 255.0F;
 
  std::istringstream gs(clean_hex.substr(2, 2));
  gs >> std::hex >> g_int;
  g = static_cast<float>(g_int) / 255.0F;
 
  std::istringstream bs(clean_hex.substr(4, 2));
  bs >> std::hex >> b_int;
  b = static_cast<float>(b_int) / 255.0F;
}

parametersCallback()

rcl_interfaces::msg::SetParametersResult lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::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 135 of file lanelet2_lichtblick_display.cpp.

                                                  {
  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'", param.get_name().c_str());
        need_republish_.store(true);
        break;
      }
    }
  }
 
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
 
  return result;
}

publishMarker()

void lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::publishMarker ( const lanelet::LaneletMapConstPtr & lanelet_map )

private

Converts a Lanelet2 map into a MarkerArray and publishes it.

This function iterates over lanelets in lanelet_map and creates markers for the various elements. The marker array is published on the marker_array_publisher_ which is configured with transient_local QoS so late joiners receive the last message.

Parameters

lanelet_map

lanelet map to visualize

Definition at line 358 of file lanelet2_lichtblick_display.cpp.

                                                                                          {
  if (lanelet_map->laneletLayer.empty()) {
    RCLCPP_WARN(this->get_logger(), "Lanelet2 map is empty.");
    return;
  }
  visualization_msgs::msg::MarkerArray marker_array_msg;
  int current_marker_id = 0;
  rclcpp::Time current_timestamp = this->now();
 
  std_msgs::msg::Header header;
  header.frame_id = map_frame_id;
  header.stamp = current_timestamp;
  float r = 0.0F;
  float g = 0.0F;
  float b = 0.0F;
 
  // Iterate through all lanelets in the map
  for (const auto& lanelet : lanelet_map->laneletLayer) {
    // --- Visualize Lines ---
    std::vector<geometry_msgs::msg::Point> left_points, right_points, centerline_points;
    for (const auto& p : lanelet.leftBound()) left_points.push_back(toRos(p.basicPoint()));
    for (const auto& p : lanelet.rightBound()) right_points.push_back(toRos(p.basicPoint()));
    for (const auto& p : lanelet.centerline()) centerline_points.push_back(toRos(p.basicPoint()));
 
    addLineStripMarker(marker_array_msg, current_marker_id, "lanelet_left_boundaries", left_bound_line_width_,
                       left_bound_color_hex_, left_bound_line_opacity_, header, left_points);
    addLineStripMarker(marker_array_msg, current_marker_id, "lanelet_right_boundaries", right_bound_line_width_,
                       right_bound_color_hex_, right_bound_line_opacity_, header, right_points);
    addLineStripMarker(marker_array_msg, current_marker_id, "lanelet_centerlines", centerline_line_width_, centerline_color_hex_,
                       centerline_line_opacity_, header, centerline_points);
 
    // --- Visualize Lanelet ID Text ---
    if (!lanelet.centerline().empty()) {
      visualization_msgs::msg::Marker text_marker;
      text_marker.header.frame_id = map_frame_id;
      text_marker.header.stamp = current_timestamp;
      text_marker.ns = "lanelet_ids";
      text_marker.id = current_marker_id++;
      text_marker.action = visualization_msgs::msg::Marker::ADD;
      text_marker.lifetime = rclcpp::Duration(std::chrono::milliseconds(0));
      text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
 
      // position the text at the geometric center of the lanelet's centerline
      geometry_msgs::msg::Point centerline_center;
      double sum_x = 0.0, sum_y = 0.0, sum_z = 0.0;
      for (const auto& p : lanelet.centerline()) {
        sum_x += p.x();
        sum_y += p.y();
        sum_z += p.z();
      }
      if (!lanelet.centerline().empty()) {
        const auto centerline_size = static_cast<double>(lanelet.centerline().size());
        centerline_center.x = sum_x / centerline_size;
        centerline_center.y = sum_y / centerline_size;
        centerline_center.z = sum_z / centerline_size;
      }
      text_marker.pose.position = centerline_center;
      text_marker.scale.z = lanelet_text_scale_;  // For TEXT_VIEW_FACING, scale.z controls the character height
 
      hexToRgb(lanelet_text_color_hex_, r, g, b);
      text_marker.color.r = r;
      text_marker.color.g = g;
      text_marker.color.b = b;
      text_marker.color.a = static_cast<float>(lanelet_text_opacity_);
 
      text_marker.text = std::to_string(lanelet.id());
 
      marker_array_msg.markers.push_back(text_marker);
    }
 
    const auto& regulatory_elements = lanelet.regulatoryElements();
    for (const auto& regulatory_element : regulatory_elements) {
      // Check if the element is a reference line
      if (auto reference_line = regulatoryElementReferenceLine(regulatory_element)) {
        std::vector<geometry_msgs::msg::Point> ref_points = {reference_line->at(0), reference_line->at(1)};
        addLineStripMarker(marker_array_msg, current_marker_id, "reference_lines", reference_line_width_,
                           reference_line_color_hex_, reference_line_opacity_, header, ref_points);
 
        // Common regulatory element logic
        // https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/RegulatoryElementTagging.md
        std::string subtype = regulatory_element->attribute("subtype").value();
        double dx = reference_line->at(1).x - reference_line->at(0).x;
        double dy = reference_line->at(1).y - reference_line->at(0).y;
        double yaw_ref_line = std::atan2(dy, dx);
        geometry_msgs::msg::Point middle_point;
        middle_point.x = (reference_line->at(0).x + reference_line->at(1).x) / 2.0;
        middle_point.y = (reference_line->at(0).y + reference_line->at(1).y) / 2.0;
 
        std::vector<geometry_msgs::msg::Point> positions = regulatoryElementPositions(regulatory_element);
        for (const auto& p : positions) {
          if (subtype == "traffic_light") {
            addMeshMarker(marker_array_msg, current_marker_id, "traffic_lights", traffic_light_mesh_resource_, p,
                          traffic_light_scale_, traffic_light_z_offset_, traffic_light_opacity_, header, yaw_ref_line,
                          middle_point);
          } else if (subtype == "right_of_way") {
            addMeshMarker(marker_array_msg, current_marker_id, "yield_signs", yield_sign_mesh_resource_, p, yield_sign_scale_,
                          yield_sign_z_offset_, yield_sign_opacity_, header, yaw_ref_line, middle_point);
          }
        }
      }
    }
  }
 
  // Publish the marker array
  marker_array_publisher_->publish(marker_array_msg);
}

regulatoryElementPositions()

std::vector< geometry_msgs::msg::Point > lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::regulatoryElementPositions ( const std::shared_ptr< const lanelet::RegulatoryElement > & regulatory_element )

staticprivate

Extracts the sign/signal positions of a regulatory element.

Only the first point of referenced line strings is considered.

Parameters

[in]

regulatory_element

regulatory element

Returns

positions

Definition at line 321 of file lanelet2_lichtblick_display.cpp.

                                                                             {
  std::vector<geometry_msgs::msg::Point> positions;
  const std::vector<lanelet::ConstLineString3d> sign_lines =
      regulatory_element->getParameters<lanelet::ConstLineString3d>(lanelet::RoleName::Refers);
  for (const auto& const_sign_line : sign_lines) {
    const std::vector<Eigen::Vector3d> sign_line = const_sign_line.basicLineString();
    if (!sign_line.empty()) {
      positions.push_back(toRos(sign_line.front()));
    }
  }
  return positions;
}

regulatoryElementReferenceLine()

std::optional< std::array< geometry_msgs::msg::Point, 2 > > lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::regulatoryElementReferenceLine ( const std::shared_ptr< const lanelet::RegulatoryElement > & regulatory_element )

staticprivate

Extracts the reference/effect line of a regulatory element.

Only the first reference line of the regulatory element is considered. Only the end points of that reference line are considered.

Parameters

[in]

regulatory_element

regulatory element

Returns

reference line

Definition at line 306 of file lanelet2_lichtblick_display.cpp.

                                                                             {
  const std::vector<lanelet::ConstLineString3d> reference_lines =
      regulatory_element->getParameters<lanelet::ConstLineString3d>(lanelet::RoleName::RefLine);
  if (reference_lines.empty()) {
    return std::nullopt;
  }
  const std::vector<Eigen::Vector3d> reference_line = reference_lines.front().basicLineString();
  if (reference_line.size() < 2) {
    return std::nullopt;
  }
  std::array<geometry_msgs::msg::Point, 2> reference_line_ros = {toRos(reference_line.front()), toRos(reference_line.back())};
  return reference_line_ros;
}

setup()

void lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::setup ( )

private

Sets up subscribers, publishers, etc. to configure the node.

Definition at line 154 of file lanelet2_lichtblick_display.cpp.

                                      {
  // callback for dynamic parameter configuration
  parameters_callback_ = this->add_on_set_parameters_callback(
      std::bind(&Lanelet2LichtblickDisplay::parametersCallback, this, std::placeholders::_1));
 
  // publisher for visualization markers
  auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
  marker_array_publisher_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/lichtblick_lanelet2_map", qos);
  RCLCPP_INFO(this->get_logger(), "Publishing to '%s' with transient_local QoS", marker_array_publisher_->get_topic_name());
 
  lanelet2_map_interface_ = std::make_shared<Lanelet2MapInterface>(*this, "lanelet2_map_server");
 
  // periodically check for map updates
  timer_ = this->create_wall_timer(std::chrono::milliseconds(2000), std::bind(&Lanelet2LichtblickDisplay::checkMapStatus, this));
}

toRos()

geometry_msgs::msg::Point lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::toRos ( const Eigen::Vector3d & point )

staticprivate

Converts an Eigen::Vector3d into a ROS geometry_msgs::msg::Point.

Simple helper to copy x, y, z components from Eigen to the ROS message.

Parameters

point

Eigen 3D point

Returns

geometry_msgs::msg::Point ROS point message

Definition at line 200 of file lanelet2_lichtblick_display.cpp.

                                                                                 {
  geometry_msgs::msg::Point ros_point;
  ros_point.x = point.x();
  ros_point.y = point.y();
  ros_point.z = point.z();
  return ros_point;
}

Member Data Documentation

auto_reconfigurable_params_

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

private

Auto-reconfigurable parameters for dynamic reconfiguration.

Definition at line 208 of file lanelet2_lichtblick_display.hpp.

centerline_color_hex_

std::string lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::centerline_color_hex_ = "#008000"

private

Definition at line 240 of file lanelet2_lichtblick_display.hpp.

centerline_line_opacity_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::centerline_line_opacity_ = 0.4

private

Definition at line 241 of file lanelet2_lichtblick_display.hpp.

centerline_line_width_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::centerline_line_width_ = 0.08

private

Definition at line 239 of file lanelet2_lichtblick_display.hpp.

current_timestamp

rclcpp::Time lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::current_timestamp

private

Definition at line 228 of file lanelet2_lichtblick_display.hpp.

lanelet2_map_interface_

std::shared_ptr<Lanelet2MapInterface> lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::lanelet2_map_interface_

private

Definition at line 224 of file lanelet2_lichtblick_display.hpp.

lanelet_text_color_hex_

std::string lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::lanelet_text_color_hex_ = "#000000"

private

Definition at line 244 of file lanelet2_lichtblick_display.hpp.

lanelet_text_opacity_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::lanelet_text_opacity_ = 1.0

private

Definition at line 245 of file lanelet2_lichtblick_display.hpp.

lanelet_text_scale_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::lanelet_text_scale_ = 0.5

private

Definition at line 243 of file lanelet2_lichtblick_display.hpp.

last_map_ptr_

lanelet::LaneletMapConstPtr lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::last_map_ptr_

private

Definition at line 225 of file lanelet2_lichtblick_display.hpp.

left_bound_color_hex_

std::string lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::left_bound_color_hex_ = "#0000FF"

private

Definition at line 232 of file lanelet2_lichtblick_display.hpp.

left_bound_line_opacity_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::left_bound_line_opacity_ = 0.5

private

Definition at line 233 of file lanelet2_lichtblick_display.hpp.

left_bound_line_width_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::left_bound_line_width_ = 0.1

private

Definition at line 231 of file lanelet2_lichtblick_display.hpp.

map_frame_id

std::string lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::map_frame_id

private

Definition at line 226 of file lanelet2_lichtblick_display.hpp.

marker_array_publisher_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::marker_array_publisher_

private

Publisher Marker Array.

Definition at line 218 of file lanelet2_lichtblick_display.hpp.

need_republish_

std::atomic<bool> lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::need_republish_ {false}

private

Definition at line 222 of file lanelet2_lichtblick_display.hpp.

{false};

parameters_callback_

OnSetParametersCallbackHandle::SharedPtr lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::parameters_callback_

private

Callback handle for dynamic parameter reconfiguration.

Definition at line 213 of file lanelet2_lichtblick_display.hpp.

reference_line_color_hex_

std::string lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::reference_line_color_hex_ = "#FFFF00"

private

Definition at line 248 of file lanelet2_lichtblick_display.hpp.

reference_line_opacity_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::reference_line_opacity_ = 0.5

private

Definition at line 249 of file lanelet2_lichtblick_display.hpp.

reference_line_width_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::reference_line_width_ = 0.2

private

Definition at line 247 of file lanelet2_lichtblick_display.hpp.

right_bound_color_hex_

std::string lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::right_bound_color_hex_ = "#FF0000"

private

Definition at line 236 of file lanelet2_lichtblick_display.hpp.

right_bound_line_opacity_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::right_bound_line_opacity_ = 0.5

private

Definition at line 237 of file lanelet2_lichtblick_display.hpp.

right_bound_line_width_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::right_bound_line_width_ = 0.1

private

Definition at line 235 of file lanelet2_lichtblick_display.hpp.

timer_

rclcpp::TimerBase::SharedPtr lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::timer_

private

Definition at line 220 of file lanelet2_lichtblick_display.hpp.

traffic_light_mesh_resource_

std::string lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::traffic_light_mesh_resource_

private

Definition at line 251 of file lanelet2_lichtblick_display.hpp.

traffic_light_opacity_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::traffic_light_opacity_ = 1.0

private

Definition at line 254 of file lanelet2_lichtblick_display.hpp.

traffic_light_scale_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::traffic_light_scale_ = 1.0

private

Definition at line 252 of file lanelet2_lichtblick_display.hpp.

traffic_light_z_offset_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::traffic_light_z_offset_ = 1.3

private

Definition at line 253 of file lanelet2_lichtblick_display.hpp.

yield_sign_mesh_resource_

std::string lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::yield_sign_mesh_resource_

private

Definition at line 256 of file lanelet2_lichtblick_display.hpp.

yield_sign_opacity_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::yield_sign_opacity_ = 1.0

private

Definition at line 259 of file lanelet2_lichtblick_display.hpp.

yield_sign_scale_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::yield_sign_scale_ = 1.0

private

Definition at line 257 of file lanelet2_lichtblick_display.hpp.

yield_sign_z_offset_

double lanelet2_lichtblick_display::Lanelet2LichtblickDisplay::yield_sign_z_offset_ = 0.0

private

Definition at line 258 of file lanelet2_lichtblick_display.hpp.

---

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

• lanelet2_lichtblick_display/include/lanelet2_lichtblick_display/lanelet2_lichtblick_display.hpp
• lanelet2_lichtblick_display/src/lanelet2_lichtblick_display.cpp