From 7c52f2ff3246637a509d079f2b2a76dcdabef135 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <mxgrey@intrinsic.ai>
Date: Sun, 26 Nov 2023 13:40:37 +0000
Subject: [PATCH] Add merge radius option for vertices

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>
---
 rmf_traffic_editor/gui/level.cpp  |  1 +
 rmf_traffic_editor/gui/vertex.cpp | 26 ++++++++++++++++++++++++++
 rmf_traffic_editor/gui/vertex.h   |  3 +++
 3 files changed, 30 insertions(+)

diff --git a/rmf_traffic_editor/gui/level.cpp b/rmf_traffic_editor/gui/level.cpp
index 1761105fe..7190ba367 100644
--- a/rmf_traffic_editor/gui/level.cpp
+++ b/rmf_traffic_editor/gui/level.cpp
@@ -1184,6 +1184,7 @@ void Level::draw(
     v.draw(
       scene,
       vertex_radius / drawing_meters_per_pixel,
+      drawing_meters_per_pixel,
       vertex_name_font,
       coordinate_system);
 
diff --git a/rmf_traffic_editor/gui/vertex.cpp b/rmf_traffic_editor/gui/vertex.cpp
index f9a9c8f93..08dd81948 100644
--- a/rmf_traffic_editor/gui/vertex.cpp
+++ b/rmf_traffic_editor/gui/vertex.cpp
@@ -21,6 +21,8 @@
 #include <QGraphicsSimpleTextItem>
 #include <QIcon>
 
+#include <iostream>
+
 #include "vertex.h"
 using std::string;
 using std::vector;
@@ -40,6 +42,7 @@ const vector<pair<string, Param::Type>> Vertex::allowed_params
   { "is_passthrough_point", Param::Type::BOOL },
   { "human_goal_set_name", Param::Type::STRING },
   { "mutex", Param::Type::STRING },
+  { "merge_radius", Param::Type::DOUBLE },
 };
 
 
@@ -132,6 +135,7 @@ YAML::Node Vertex::to_yaml(const CoordinateSystem& coordinate_system) const
 void Vertex::draw(
   QGraphicsScene* scene,
   const double radius,
+  const double drawing_meters_per_pixel,
   const QFont& font,
   const CoordinateSystem& coordinate_system) const
 {
@@ -164,6 +168,19 @@ void Vertex::draw(
   const double icon_ring_radius = radius * 2.5;
   const double icon_scale = 2.0 * radius / 128.0;
 
+  if (const auto r_merge_opt = merge_radius())
+  {
+    const double r_merge = *r_merge_opt / drawing_meters_per_pixel;
+    const QPen radius_pen(selected ? selected_color : Qt::black);
+    auto* radius_item = scene->addEllipse(
+      x - r_merge,
+      y - r_merge,
+      2 * r_merge,
+      2 * r_merge,
+      radius_pen);
+    radius_item->setZValue(19.0);
+  }
+
   if (is_holding_point())
   {
     const double icon_bearing = -135.0 * M_PI / 180.0;
@@ -343,6 +360,15 @@ bool Vertex::is_charger() const
   return it->second.value_bool;
 }
 
+std::optional<double> Vertex::merge_radius() const
+{
+  const auto it = params.find("merge_radius");
+  if (it == params.end())
+    return std::nullopt;
+
+  return it->second.value_double;
+}
+
 bool Vertex::is_cleaning_zone() const
 {
   const auto it = params.find("is_cleaning_zone");
diff --git a/rmf_traffic_editor/gui/vertex.h b/rmf_traffic_editor/gui/vertex.h
index 9b2214ced..c4ab60ac6 100644
--- a/rmf_traffic_editor/gui/vertex.h
+++ b/rmf_traffic_editor/gui/vertex.h
@@ -22,6 +22,7 @@
 #include <map>
 #include <string>
 #include <vector>
+#include <optional>
 #include <yaml-cpp/yaml.h>
 
 #include <QColor>
@@ -58,6 +59,7 @@ class Vertex
   void draw(
     QGraphicsScene* scene,
     const double radius,
+    const double drawing_meters_per_pixel,
     const QFont& font,
     const CoordinateSystem& coordinate_system) const;
 
@@ -65,6 +67,7 @@ class Vertex
   bool is_holding_point() const;
   bool is_cleaning_zone() const;
   bool is_charger() const;
+  std::optional<double> merge_radius() const;
 
   std::string dropoff_ingestor() const;
   std::string pickup_dispenser() const;