lanelet2_lichtblick_display.hpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: Apache-2.0
 
#pragma once
 
#include <atomic>
#include <cmath>
#include <functional>
#include <memory>
#include <string>
#include <vector>
 
#include <tf2/LinearMath/Quaternion.h>
#include <rclcpp/rclcpp.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
#include "geometry_msgs/msg/point.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
 
#include "lanelet2_map_interface/lanelet2_map_interface.hpp"
 
namespace lanelet2_lichtblick_display {
 
template <typename C>
struct is_vector : std::false_type {};
template <typename T, typename A>
struct is_vector<std::vector<T, A>> : std::true_type {};
template <typename C>
inline constexpr bool is_vector_v = is_vector<C>::value;
 
class Lanelet2LichtblickDisplay : public rclcpp::Node {
 public:
  Lanelet2LichtblickDisplay();
 
 private:
  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 = "");
 
  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter>& parameters);
 
  void setup();
 
  void checkMapStatus();
 
  void hexToRgb(const std::string& hex, float& r, float& g, float& b);
 
  void publishMarker(const lanelet::LaneletMapConstPtr& lanelet_map);
 
  static geometry_msgs::msg::Point toRos(const Eigen::Vector3d& point);
 
  void clearAllMarkers();
 
  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);
 
  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);
 
  static std::optional<std::array<geometry_msgs::msg::Point, 2>> regulatoryElementReferenceLine(
      const std::shared_ptr<const lanelet::RegulatoryElement>& regulatory_element);
 
  static std::vector<geometry_msgs::msg::Point> regulatoryElementPositions(
      const std::shared_ptr<const lanelet::RegulatoryElement>& regulatory_element);
 
  std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)>>> auto_reconfigurable_params_;
 
  OnSetParametersCallbackHandle::SharedPtr parameters_callback_;
 
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_array_publisher_;
 
  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;
 
  // Parameters
  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;
};
 
}  // namespace lanelet2_lichtblick_display