marker.hpp

Go to the documentation of this file.

#pragma once
 
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
 
#include <map>
#include <type_traits>
 
#include "common_lib/structures/cone.hpp"
#include "common_lib/structures/path_point.hpp"
#include "common_lib/structures/position.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/color_rgba.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
 
using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using Point = K::Point_2;
namespace common_lib::communication {
 
inline const std::map<std::string, std::array<float, 4>, std::less<>>& marker_color_map() {
  static const std::map<std::string, std::array<float, 4>, std::less<>> map = {
      {"blue", {0.0f, 0.0f, 1.0f, 1.0f}},   {"yellow", {1.0f, 1.0f, 0.0f, 1.0f}},
      {"orange", {1.0f, 0.5f, 0.0f, 1.0f}}, {"red", {1.0f, 0.0f, 0.0f, 1.0f}},
      {"green", {0.0f, 1.0f, 0.0f, 1.0f}},  {"white", {1.0f, 1.0f, 1.0f, 1.0f}},
      {"grey", {0.5f, 0.5f, 0.5f, 1.0f}},
  };
  return map;
}
 
inline const std::map<std::string, int, std::less<>>& marker_shape_map() {
  static const std::map<std::string, int, std::less<>> map = {
      {"cylinder", visualization_msgs::msg::Marker::CYLINDER},
      {"cube", visualization_msgs::msg::Marker::CUBE},
      {"sphere", visualization_msgs::msg::Marker::SPHERE},
      {"line", visualization_msgs::msg::Marker::LINE_STRIP},
      {"cone", visualization_msgs::msg::Marker::MESH_RESOURCE},
      {"arrow", visualization_msgs::msg::Marker::ARROW},
  };
  return map;
}
 
template <typename T, typename = void>
struct HasPosition : public std::false_type {};
 
template <typename T>
struct HasPosition<T,
                   std::enable_if_t<std::is_arithmetic_v<decltype(std::declval<T>().position.x)> &&
                                    std::is_arithmetic_v<decltype(std::declval<T>().position.y)>>>
    : public std::true_type {};
 
template <typename T>
visualization_msgs::msg::MarkerArray marker_array_from_structure_array(
    const std::vector<T>& structure_array, const std::string& name_space,
    const std::string& frame_id, const std::string& color = "red",
    const std::string& shape = "cylinder", float scale = 0.5,
    int action = visualization_msgs::msg::Marker::MODIFY) {
  static_assert(
      HasPosition<T>::value,
      "Template argument T must have a data member named 'position' with 'x' and 'y' sub-members");
 
  visualization_msgs::msg::MarkerArray marker_array;
  std::array<float, 4> color_array = marker_color_map().at(color);
 
  for (size_t i = 0; i < structure_array.size(); ++i) {
    visualization_msgs::msg::Marker marker;
 
    marker.header.frame_id = frame_id;
    marker.header.stamp = rclcpp::Clock().now();
    marker.ns = name_space;
    marker.id = static_cast<int>(i);
    marker.type = marker_shape_map().at(shape);
    marker.action = action;
 
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
 
    marker.pose.position.x = structure_array[i].position.x;
    marker.pose.position.y = structure_array[i].position.y;
    marker.pose.position.z = 0;
 
    marker.scale.x = scale;
    marker.scale.y = scale;
    marker.scale.z = scale;
 
    marker.color.r = color_array[0];
    marker.color.g = color_array[1];
    marker.color.b = color_array[2];
    marker.color.a = color_array[3];
 
    /* path points in planning also use this and have no islarge.
    if (shape == "cylinder") {
      if (structure_array[i].is_large) {
        marker.scale.x = 1.5 * scale;
        marker.scale.y = 1.5 * scale;
        marker.scale.z = 1.5 * scale;
      }
    }
    */
 
    if (shape == "cone") {
      marker.pose.orientation.x = 0.7071;  // Approximately sqrt(2)/2
      marker.pose.orientation.y = 0.0;
      marker.pose.orientation.z = 0.0;
      marker.pose.orientation.w = -0.7071;  // Approximately sqrt(2)/2
      marker.scale.x = scale * 0.03;
      marker.scale.y = scale * 0.03;
      marker.scale.z = scale * 0.03;
      marker.mesh_resource = "https://paginas.fe.up.pt/~up202109860/FormulaStudent/FSGConev2.obj";
    }
    marker_array.markers.push_back(marker);
  }
 
  return marker_array;
}
 
template <typename T>
visualization_msgs::msg::Marker line_marker_from_structure_array(
    const std::vector<T>& structure_array, const std::string& name_space,
    const std::string& frame_id, const int id, const std::string& color = "red",
    const std::string& shape = "line", float scale = 0.1f,
    int action = visualization_msgs::msg::Marker::MODIFY) {
  static_assert(
      HasPosition<T>::value,
      "Template argument T must have a data member named 'position' with 'x' and 'y' sub-members");
 
  std::array<float, 4> color_array = marker_color_map().at(color);
 
  visualization_msgs::msg::Marker marker;
 
  marker.header.frame_id = frame_id;
  marker.header.stamp = rclcpp::Clock().now();
  marker.ns = name_space;
  marker.id = id;
  marker.type = marker_shape_map().at(shape);
  marker.action = action;
 
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
 
  marker.pose.position.x = 0;
  marker.pose.position.y = 0;
  marker.pose.position.z = 0;
 
  marker.scale.x = scale;
  marker.scale.y = scale;
  marker.scale.z = scale;
 
  marker.color.r = color_array[0];
  marker.color.g = color_array[1];
  marker.color.b = color_array[2];
  marker.color.a = color_array[3];
 
  for (size_t i = 0; i < structure_array.size(); ++i) {
    geometry_msgs::msg::Point point;
    point.x = structure_array[i].position.x;
    point.y = structure_array[i].position.y,
 
    marker.points.push_back(point);
  }
 
  return marker;
}
 
visualization_msgs::msg::Marker marker_from_position(
    const common_lib::structures::Position& position, const std::string& name_space, const int id,
    const std::string& color = "red", float scale = 0.5, const std::string& frame_id = "map",
    const std::string& shape = "sphere", int action = visualization_msgs::msg::Marker::ADD);
 
visualization_msgs::msg::Marker lines_marker_from_triangulations(
    const std::vector<std::pair<Point, Point>>& triangulations, const std::string& name_space,
    const std::string& frame_id, int id, const std::string& color, float scale, int action);
 
visualization_msgs::msg::MarkerArray velocity_hover_markers(
    const std::vector<common_lib::structures::PathPoint>& path_array, const std::string& name_space,
    const std::string& frame_id, float scale = 0.2f, int every_nth = 1);
}  // namespace common_lib::communication