Added robot and things

.clang-format

@@ -0,0 +1,68 @@
+---
+Language: Cpp
+BasedOnStyle:  Mozilla
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveMacros: true
+AlignConsecutiveAssignments: true
+AlignEscapedNewlines: Right
+AlignOperands: false
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: true
+AllowAllConstructorInitializersOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: true
+AllowShortCaseLabelsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: true
+AllowShortIfStatementsOnASingleLine: Never
+AllowShortLambdasOnASingleLine: All
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: Yes
+BreakBeforeBinaryOperators: true
+BreakBeforeBraces: Attach
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializers: AfterColon
+ColumnLimit: 120
+CompactNamespaces: false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: false
+IncludeBlocks: Preserve
+IndentCaseLabels: true
+IndentWidth: 4
+PointerAlignment: Left
+PackConstructorInitializers: NextLine
+ReflowComments: false
+SortIncludes: false
+SortUsingDeclarations: false
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyBlock: true
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 1
+SpacesInAngles: false
+SpacesInCStyleCastParentheses: false
+SpacesInContainerLiterals: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Auto
+TabWidth: 4
+UseTab: Never
+
+AllowShortEnumsOnASingleLine: false
+
+BraceWrapping:
+  AfterEnum: false
+
+AlignConsecutiveDeclarations: 
+  Enabled: true
+
+NamespaceIndentation: All

.clang-tidy

@@ -0,0 +1,34 @@
+---
+Checks: "
+        *,
+        -abseil-*,
+        -altera-*,
+        -android-*,
+        -fuchsia-*,
+        -google-*,
+        -llvm*,
+        -modernize-use-trailing-return-type,
+        -zircon-*,
+        -readability-else-after-return,
+        -readability-static-accessed-through-instance,
+        -readability-avoid-const-params-in-decls,
+        -cppcoreguidelines-non-private-member-variables-in-classes,
+        -misc-non-private-member-variables-in-classes,
+        -misc-non-private-member-variables-in-classes,
+        -readability-identifier-length,
+        -misc-include-cleaner,
+        -modernize-avoid-bind,
+        -hicpp-*,
+        -bugprone-easily-swappable-parameters,
+        -readability-function-cognitive-complexity,
+        -readability-named-parameter,
+        -cppcoreguidelines-pro-type-union-access,
+        -cppcoreguidelines-owning-memory,
+        -cppcoreguidelines-macro-usage,
+        -performance-unnecessary-value-param,
+        -modernize-use-nodiscard,
+        -cppcoreguidelines-pro-bounds-constant-array-index,
+    "
+WarningsAsErrors: ''
+HeaderFilterRegex: ''
+FormatStyle:     none

.clangd

@@ -0,0 +1,6 @@
+---
+Diagnostics:
+  UnusedIncludes: None 
+  MissingIncludes: None
+  ClangTidy:
+    FastCheckFilter: Loose

CMakeLists.txt

@@ -12,10 +12,17 @@ find_package(pluginlib REQUIRED)
 pluginlib_export_plugin_description_file(rqt_gui "plugin.xml")
 find_package(rqt_gui_cpp REQUIRED)
 find_package(qt_gui_cpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(Qt5 COMPONENTS Core REQUIRED)
 find_package(Qt5 COMPONENTS Widgets REQUIRED)
+find_package(Qt5 COMPONENTS Svg REQUIRED)
 find_package(Qt5 COMPONENTS Qml REQUIRED)
 find_package(Qt5 COMPONENTS Quick REQUIRED)
+find_package(Qt5 COMPONENTS QuickControls2 REQUIRED)
+find_package(mg_msgs REQUIRED)
 find_package(Qt5QuickCompiler)
 
 set(CMAKE_AUTOMOC ON)
@@ -50,10 +57,17 @@ ament_target_dependencies(
   pluginlib
   rqt_gui_cpp
   qt_gui_cpp
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
   Qt5Core
   Qt5Widgets
+  Qt5Svg
   Qt5Qml
   Qt5Quick
+  Qt5QuickControls2
+  mg_msgs
 )
 
 install(

include/rqt_demo_plugin/lineItem.hpp

@@ -1,22 +1,14 @@
 #pragma once
-#include <QtQuick/QQuickItem>
-#include <qchar.h>
-#include <qglobal.h>
-#include <qobject.h>
-#include <qobjectdefs.h>
-#include <qpoint.h>
-#include <qquickitem.h>
-#include <qsgnode.h>
 
-class LineItem : public QQuickItem
+#include <QtQuick/QQuickItem>
-{
+
+class LineItem : public QQuickItem {
     Q_OBJECT
 
     Q_PROPERTY(float scale READ scale WRITE setScale NOTIFY scaleChanged)
     Q_PROPERTY(QString stupid READ stupid WRITE setStupid NOTIFY stupidChanged)
     Q_PROPERTY(QObject* handler READ handler WRITE setHandler NOTIFY handlerChanged)
 
-
     QML_ELEMENT
   public:
     LineItem(QQuickItem* parent = nullptr);
@@ -29,14 +21,12 @@ public:
 
     void setStupid(const QString& str) { stupid_ = str; }
     void setScale(const float scale) { scale_ = scale; }
-    void setHandler(QObject* const obj) {handler_ = obj;}
+    void setHandler(QObject* const obj);
 
   signals:
     void stupidChanged(const QString& str);
     void handlerChanged(QObject* const obj);
 
-    
-
   private:
     float    scale_;
     QString  stupid_;

include/rqt_demo_plugin/rqt_demo_plugin.hpp

@@ -1,20 +1,24 @@
 #pragma once
 
+#include "geometry_msgs/msg/point.hpp"
 #include "rqt_gui_cpp/plugin.h"
+#include "mg_msgs/msg/t_edit_cycle.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
 
 #include <QtGui>
 #include <QtQuick>
+#include <QtQuickControls2/QQuickStyle>
+
 #include <QtWidgets>
-#include <qfileinfo.h>
+#include <chrono>
-#include <qobject.h>
+#include <functional>
-#include <qobjectdefs.h>
+#include <memory>
-#include <qpushbutton.h>
+#include <rclcpp/publisher.hpp>
-#include <qquickview.h>
+#include <rclcpp/qos.hpp>
-#include <qquickwindow.h>
+#include <unordered_set>
-#include <qresource.h>
+#include <vector>
-#include <qsggeometry.h>
-#include <qurl.h>
-#include <qwidget.h>
 
 inline void initResources() { Q_INIT_RESOURCE(res); }
 inline void cleanupResources() { Q_CLEANUP_RESOURCE(res); }
@@ -24,38 +28,77 @@ namespace mg {
     class DemoPluginMg : public rqt_gui_cpp::Plugin {
         Q_OBJECT
         Q_PROPERTY(QObject* demoPlugin READ demoPlugin NOTIFY demoPluginChanged)
+        Q_PROPERTY(QPointF robotPos READ robotPos NOTIFY robotPosChanged)
+        Q_PROPERTY(qreal robotTheta READ robotTheta NOTIFY robotThetaChanged)
+
       public:
+        std::mutex tree_mut_;
+        bool       children_dirty;
+        bool       points_dirty;
+
         DemoPluginMg();
 
-  virtual void shutdownPlugin() {
+        void initPlugin(qt_gui_cpp::PluginContext&) override;
-    cleanupResources();
+        void shutdownPlugin() override { cleanupResources(); }
-  }
 
-  virtual void initPlugin(qt_gui_cpp::PluginContext &);
+        Q_INVOKABLE void postPoint(qreal x, qreal y);
 
-  QObject * demoPlugin() const {
+        QObject* demoPlugin() const { return (QObject*)(this); }
-    return (QObject *)(this);
+        QPointF  robotPos() const { return robotPos_; }
-  }
+        float    robotTheta() const { return robotTheta_; }
 
-  const QSGGeometry::Point2D hello[8] = {
+        const std::vector<std::unordered_set<int>>& children() const { return children_; }
-    {0.2,0.2}, {0.8,0.2},
+        const std::vector<std::pair<float, float>>& points() const { return points_; }
-    {0.2,0.2}, {0.2,0.8},
-    {0.2,0.4}, {0.4,0.8},
-    {0.2,0.4}, {0.1,0.1},
-  };
 
-  const int hello_c = 8;
+        int cchildren() const { return cchildren_; };
 
       signals:
 
         void demoPluginChanged(QObject* const);
+        void robotPosChanged(QPointF const);
+        void robotThetaChanged(float const);
 
+        void treeChanged();
+
+      private:
+        std::shared_ptr<rclcpp::Subscription<mg_msgs::msg::TEditCycle>> editcycle_sub_;
+
+        std::vector<std::unordered_set<int>> children_;
+        std::vector<std::pair<float, float>> points_;
+
+        std::shared_ptr<rclcpp::TimerBase>          tf_timer_;
+        std::shared_ptr<tf2_ros::Buffer>            tf_buffer_;
+        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+        rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr pub_point_;
+
+        QPointF robotPos_;
+        qreal   robotTheta_;
+
+        int cchildren_;
+
+        void cb_GraphEdits(mg_msgs::msg::TEditCycle::ConstSharedPtr msg);
+        void cb_tf_timer();
     };
 
     inline void DemoPluginMg::initPlugin(qt_gui_cpp::PluginContext& pc) {
         initResources();
+
+        tf_buffer_   = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+        tf_timer_  = node_->create_timer(std::chrono::milliseconds(15), [this]() { this->cb_tf_timer(); });
+        pub_point_ = node_->create_publisher<geometry_msgs::msg::PointStamped>("/clicked_point", rclcpp::QoS(1));
+
+        QQuickStyle::setStyle("fusion");
+
         const QUrl qrl("qrc:/qml/helloworld.qml");
-  QQuickView *qv = new QQuickView();
+        auto*      qv = new QQuickView();
+
+        auto editcycle_cb = std::bind(&DemoPluginMg::cb_GraphEdits, this, std::placeholders::_1);
+        editcycle_sub_    = node_->create_subscription<mg_msgs::msg::TEditCycle>(
+          "GraphEdits", rclcpp::QoS(1000).transient_local(), editcycle_cb);
+
         qv->rootContext()->setContextProperty("DemoPlug", this);
         qv->setSource(qrl);
         qv->setResizeMode(QQuickView::ResizeMode::SizeRootObjectToView);
@@ -66,5 +109,4 @@ inline void DemoPluginMg::initPlugin(qt_gui_cpp::PluginContext &pc) {
         pc.addWidget(qw_);
     }
 
-
 }; // namespace mg

mg_msgs/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.8)
+project(mg_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Point2d.msg"
+  "msg/TEdit.msg"
+  "msg/TEditCycle.msg"
+  "action/MoveStraight.action"
+  "action/MovePoint.action"
+  "action/LookAt.action"
+  "action/Rotate.action"
+  "srv/SetWidth.srv"
+)
+
+ament_package()

mg_msgs/action/LookAt.action

@@ -0,0 +1,25 @@
+float64 x
+float64 y
+
+float64 step 0.01
+float64 tolerance 0.001
+float64 max_wheel_speed 3
+float64 max_angular 3.14
+float64 max_vel 2
+float64 pos_mult 15
+float64 ornt_mult 4
+float64 t_ornt_mult 0.1
+float64 obst_mult_a -5
+float64 obst_mult_b -30
+float64 obst_mult_c -9
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+uint8 MISALIGNED=3
+uint8 UNKNOWN=254
+
+uint8 error
+---
+float64 distance_passed

mg_msgs/action/MovePoint.action

@@ -0,0 +1,25 @@
+float64 x 
+float64 y 
+
+float64 step 0.01
+float64 tolerance 0.001
+float64 max_wheel_speed 3
+float64 max_angular 3.14
+float64 max_vel 2
+float64 pos_mult 15
+float64 ornt_mult 4
+float64 t_ornt_mult 0.1
+float64 obst_mult_a -5
+float64 obst_mult_b -30
+float64 obst_mult_c -9
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+uint8 MISALIGNED=3
+uint8 UNKNOWN=254
+
+uint8 error
+---
+float64 distance_passed

mg_msgs/action/MoveStraight.action

@@ -0,0 +1,24 @@
+float64 distance
+
+float64 step 0.01
+float64 tolerance 0.001
+float64 max_wheel_speed 3
+float64 max_angular 3.14
+float64 max_vel 2
+float64 pos_mult 15
+float64 ornt_mult 4
+float64 t_ornt_mult 0.1
+float64 obst_mult_a -5
+float64 obst_mult_b -30
+float64 obst_mult_c -9
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+uint8 MISALIGNED=3
+uint8 UNKNOWN=254
+
+uint8 error
+---
+float64 distance_passed

mg_msgs/action/Rotate.action

@@ -0,0 +1,25 @@
+
+float64 angle
+
+float64 step 0.01
+float64 tolerance 0.001
+float64 max_wheel_speed 3
+float64 max_angular 3.14
+float64 max_vel 2
+float64 pos_mult 15
+float64 ornt_mult 4
+float64 t_ornt_mult 0.1
+float64 obst_mult_a -5
+float64 obst_mult_b -30
+float64 obst_mult_c -9
+string[] ignore_tags []
+---
+uint8 SUCCESS=0
+uint8 BLOCKED=1
+uint8 TIMEOUT=2
+uint8 MISALIGNED=3
+uint8 UNKNOWN=254
+
+uint8 error
+---
+float64 distance_passed

mg_msgs/msg/Point2d.msg

@@ -0,0 +1,2 @@
+float64 x
+float64 y

mg_msgs/msg/TEdit.msg

@@ -0,0 +1,4 @@
+
+uint8 type
+uint16 idx
+uint16 idy

mg_msgs/msg/TEditCycle.msg

@@ -0,0 +1,8 @@
+
+mg_msgs/TEdit[100] modifications
+mg_msgs/Point2d[100] new_points
+uint8 cmodifications 0
+uint8 cnew_points 0
+
+uint8 DELETE=1
+uint8 ADD=0

mg_msgs/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mg_msgs</name>
+  <version>0.0.0</version>
+  <description>This contains various msg/action used within the project</description>
+  <maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mg_msgs/srv/SetWidth.srv

@@ -0,0 +1,2 @@
+float64 width
+---

package.xml

@@ -14,6 +14,10 @@
   <depend>rqt</depend>
   <depend>rqt_gui_cpp</depend>
   <depend>rqt_image_view</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <depend>mg_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

resources/qml/helloworld.qml

@@ -1,22 +1,42 @@
 import QtQuick 2.3
+import QtQuick.Controls 2.0
 import LineItem 1.0
 import DemoPlugin 1.0
 
+Item {
+id: root
+Column {
+    Row {
+        id: topbar
+        Button {
+            id: targetMode
+            text: "Target"
+            flat: false 
+            checkable: true
+            onClicked: function() {}
+        }
+    }
+
     Rectangle {
+        height: root.height - topbar.height
+        width: root.width
         id: rr
-    color: "gray"
+        color: "dimgrey"
-    property var scale: 0.4
+        property var scale: 0.5
+        
         Flickable {
+            id: viewport
             width: parent.width;
             height: parent.height
-        contentWidth: 3000*rr.scale; contentHeight: 2000*rr.scale
+            contentWidth: 3000; contentHeight: 2000
             flickDeceleration: 10000
             leftMargin: Math.max((width - contentWidth)/2,0)
             topMargin: Math.max((height - contentHeight)/2,0)
+            
             clip: true
 
             Rectangle {
-            color: "white"
+                color: "lightgrey"
                 id: contentRect
                 anchors.top: parent.top
                 anchors.left: parent.left
@@ -25,6 +45,13 @@ Rectangle {
                 
                 transform: Scale {yScale: rr.scale; xScale: rr.scale}
                 transformOrigin: Item.TopLeft
+                Image {
+                    x: 0;y: 0
+                    height: 2000
+                    width: 3000
+                    source: "../playmat_grid_v1.svg"
+                    antialiasing: false
+                }
 
                 Row {
                     x:0
@@ -37,6 +64,53 @@ Rectangle {
                         handler: DemoPlug.demoPlugin
                     }
                 }
+                Image {
+                    x: DemoPlug.robotPos.x * 1000 - 140
+                    y: (2.0 - DemoPlug.robotPos.y) * 1000 - 160
+                    height: 320;
+                    width: 280;
+                    transform: [Rotation {angle: -DemoPlug.robotTheta/( Math.PI/180 ); origin.x: 140; origin.y: 160}]
+                    source: "../robot.jpg"
+                }
+            }
+            MouseArea {
+                anchors.fill: parent
+                propagateComposedEvents: true
+
+                onPressed: (e)=> {
+                    e.accepted = targetMode.checked
+                    console.log("onPressed")
+                }
+
+                onClicked: (e)=> {
+                    DemoPlug.postPoint(e.x/rr.scale/1000,2.0 - e.y/rr.scale/1000)
+                    e.accepted = true
+                    console.log("onClicked")
+                }
+                
+                onWheel: (e) => {
+                    if (e.modifiers & Qt.ControlModifier) {
+                        rr.scale+=(e.angleDelta.y / 1080);
+                        rr.scale = Math.min(Math.max(rr.scale,0.5),10.0)
+
+                        viewport.resizeContent(3000*rr.scale, 2000*rr.scale, Qt.point(e.x,e.y))
+                    } else {
+                        e.accepted = false
+                    }
+                }
+            }
+        }
+        MouseArea {
+            anchors.fill: parent
+            propagateComposedEvents: true
+
+            onPressed: (e)=> {
+                e.accepted = false
+            }
+
+            
+        }
+
     }
 }
 }

resources/res.qrc

@@ -1,5 +1,7 @@
 <!DOCTYPE RCC><RCC version="1.0">
     <qresource>
         <file>qml/helloworld.qml</file>
+        <file>robot.jpg</file>
+        <file>playmat_grid_v1.svg</file>
     </qresource>
 </RCC>

src/lineItem.cpp

@@ -1,71 +1,99 @@
 #include "rqt_demo_plugin/lineItem.hpp"
 #include "rqt_demo_plugin/rqt_demo_plugin.hpp"
+#include <mutex>
 #include <qglobal.h>
-#include <qqmlerror.h>
+#include <qobjectdefs.h>
-#include <qquickitem.h>
+#include <sys/types.h>
-#include <qsgflatcolormaterial.h>
+#include <unordered_set>
-#include <qsggeometry.h>
-#include <qsgnode.h>
-#include <qsize.h>
 
-LineItem::LineItem(QQuickItem *parent) 
+LineItem::LineItem(QQuickItem* parent) : QQuickItem(parent), scale_(1), stupid_(""), handler_(nullptr) {
-    : QQuickItem(parent)
-    , scale_(1) 
-    , stupid_("")
-    , handler_(nullptr)
-{
     setFlag(ItemHasContents, true);
 }
 
 LineItem::~LineItem() = default;
 
+void LineItem::setHandler(QObject* const obj) {
+    handler_     = obj;
+    auto* plugin = dynamic_cast<mg::DemoPluginMg*>(handler_);
+    connect(plugin, SIGNAL(treeChanged()), this, SLOT(update()));
+    emit handlerChanged(obj);
+    update();
+}
 
-QSGNode *LineItem::updatePaintNode(QSGNode *old, UpdatePaintNodeData *) 
+QSGNode* LineItem::updatePaintNode(QSGNode* old, UpdatePaintNodeData*) {
-{
     QSGGeometryNode* node     = nullptr;
     QSGGeometry*     geometry = nullptr;
-    mg::DemoPluginMg *plugin = static_cast<mg::DemoPluginMg *>(handler_);
+    auto*            plugin   = dynamic_cast<mg::DemoPluginMg*>(handler_);
+
     if (!plugin) {
         qmlEngine(this)->throwError("Handler plugin was not provided");
         return old;
     }
+    if (plugin->cchildren() == 0) {
+        return nullptr;
+    }
 
-    const int hello_c = plugin->hello_c; 
+    std::lock_guard lock(plugin->tree_mut_);
 
     if (!old) {
+        geometry
+          = new QSGGeometry(QSGGeometry::defaultAttributes_Point2D(), plugin->points().size(), 2 * plugin->cchildren());
+
         node           = new QSGGeometryNode;
-        geometry = new QSGGeometry(QSGGeometry::defaultAttributes_Point2D(), hello_c);
-        geometry->setLineWidth(3*scale_);
-        geometry->setDrawingMode(QSGGeometry::DrawLines);
-        geometry->setVertexDataPattern(QSGGeometry::DynamicPattern);
-        geometry->setIndexDataPattern(QSGGeometry::DynamicPattern);
-        node->setGeometry(geometry);
-        node->setFlag(QSGNode::OwnsGeometry);
         auto* material = new QSGFlatColorMaterial;
+
+        geometry->setDrawingMode(QSGGeometry::DrawLines);
+
+        geometry->setVertexDataPattern(QSGGeometry::StaticPattern);
+        geometry->setIndexDataPattern(QSGGeometry::DynamicPattern);
         material->setColor(QColor(255, 0, 0));
+
+        node->setGeometry(geometry);
         node->setMaterial(material);
+        node->setFlag(QSGNode::OwnsGeometry);
         node->setFlag(QSGNode::OwnsMaterial);
-
     } else {
-        node = static_cast<QSGGeometryNode *>(old);
+        node     = dynamic_cast<QSGGeometryNode*>(old);
         geometry = node->geometry();
-        if(hello_c != prev_size) {
+
-            geometry->allocate(prev_size);
+        geometry->allocate(plugin->points().size(), 2 * plugin->cchildren());
-        }
     }
 
-
+    if (plugin->points_dirty || plugin->children_dirty) {
-    if(hello_c != prev_size) {
         const QSizeF s = size();
+
+        const std::vector<std::pair<float, float>>& points    = plugin->points();
         QSGGeometry::Point2D*                       verticies = geometry->vertexDataAsPoint2D();
-        for(int i = 0; i<hello_c; i++) {
+
-            verticies[i].x = plugin->hello[i].x * s.width();
+        int i = 0;
-            verticies[i].y = plugin->hello[i].y * s.height();
+        for (const auto& p : points) {
+            verticies[i].x = p.first * s.width();
+            verticies[i].y = p.second * s.height();
+            i++;
         }
         geometry->markVertexDataDirty();
-        node->markDirty(QSGNode::DirtyGeometry);
+        plugin->points_dirty = false;
-        prev_size = hello_c;
     }
 
+    if (plugin->children_dirty) {
+        const std::vector<std::unordered_set<int>>& children = plugin->children();
+        ushort*                                     indicies = geometry->indexDataAsUShort();
+
+        int i = 0;
+        int j = 0;
+        for (const auto& cset : children) {
+            for (const auto& child : cset) {
+                indicies[j++] = i;
+                indicies[j++] = child;
+            }
+            i++;
+        }
+        geometry->markIndexDataDirty();
+        plugin->children_dirty = false;
+    }
+
+    geometry->setLineWidth(3 * scale_);
+    node->markDirty(QSGNode::DirtyGeometry);
+
     return node;
 }

src/rqt_demo_plugin.cpp

@@ -1,21 +1,82 @@
 #include "rqt_demo_plugin/rqt_demo_plugin.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+#include "mg_msgs/msg/t_edit.hpp"
+#include "mg_msgs/msg/t_edit_cycle.hpp"
 #include "rqt_demo_plugin/lineItem.hpp"
 
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/LinearMath/Transform.h"
+#include "tf2/convert.h"
+
 #include "pluginlib/class_list_macros.hpp"
+#include <mutex>
 #include <qnamespace.h>
 #include <qqml.h>
 #include <qsurfaceformat.h>
+
 PLUGINLIB_EXPORT_CLASS(mg::DemoPluginMg, rqt_gui_cpp::Plugin)
 
-mg::DemoPluginMg::DemoPluginMg() : Plugin() {
+mg::DemoPluginMg::DemoPluginMg() :
+  children_dirty(false), points_dirty(false), robotPos_(0, 0), robotTheta_(0), cchildren_(0) {
     QSurfaceFormat fmt;
     fmt.setSwapInterval(0);
     fmt.setRenderableType(QSurfaceFormat::OpenGLES);
     QSurfaceFormat::setDefaultFormat(fmt);
-  static bool typesRegistered = 0;
+    static bool typesRegistered = false;
     if (!typesRegistered) {
         qmlRegisterType<LineItem>("LineItem", 1, 0, "LineItem");
         qmlRegisterAnonymousType<DemoPluginMg>("DemoPlugin", 1);
-    typesRegistered = 1;
+        typesRegistered = true;
     }
 }
+
+void mg::DemoPluginMg::cb_GraphEdits(mg_msgs::msg::TEditCycle::ConstSharedPtr msg) {
+    std::lock_guard lock(tree_mut_);
+    RCLCPP_INFO(node_->get_logger(), "Got edit: %u %u", msg->cnew_points, msg->cmodifications);
+    for (int i = 0; i < msg->cnew_points; i++) {
+        points_.emplace_back(msg->new_points[i].x, msg->new_points[i].y);
+        children_.emplace_back();
+    }
+    for (int i = 0; i < msg->cmodifications; i++) {
+        const mg_msgs::msg::TEdit modification = msg->modifications[i];
+        if (modification.type == mg_msgs::msg::TEditCycle::DELETE) {
+            children_[modification.idx].erase(modification.idy);
+            cchildren_--;
+        } else {
+            children_[modification.idx].insert(modification.idy);
+            cchildren_++;
+        }
+    }
+
+    points_dirty   = msg->cnew_points > 0;
+    children_dirty = msg->cmodifications > 0;
+    RCLCPP_INFO(node_->get_logger(), "Got edit: %lu %u", points_.size(), cchildren_);
+    emit treeChanged();
+}
+
+void mg::DemoPluginMg::cb_tf_timer() {
+
+    try {
+        double x = NAN;
+        double y = NAN;
+
+        auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero);
+
+        tf2::Transform t;
+        tf2::convert(tmsg.transform, t);
+        t.getBasis().getRPY(x, y, robotTheta_);
+        auto vec3 = tmsg.transform.translation;
+
+        robotPos_ = { vec3.x, vec3.y };
+        robotPosChanged(robotPos_);
+        robotThetaChanged(robotTheta_);
+    } catch (const tf2::TransformException& ex) { }
+}
+
+void mg::DemoPluginMg::postPoint(qreal x, qreal y) {
+    geometry_msgs::msg::PointStamped point;
+    point.header.frame_id = "odom";
+    point.point.x         = x;
+    point.point.y         = y;
+    pub_point_->publish(point);
+}