diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..5ffc498 --- /dev/null +++ b/.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 \ No newline at end of file diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..fa92005 --- /dev/null +++ b/.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 \ No newline at end of file diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..68e449a --- /dev/null +++ b/.clangd @@ -0,0 +1,6 @@ +--- +Diagnostics: + UnusedIncludes: None + MissingIncludes: None + ClangTidy: + FastCheckFilter: Loose \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b072ac..815ccc4 100644 --- a/CMakeLists.txt +++ b/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( diff --git a/include/rqt_demo_plugin/lineItem.hpp b/include/rqt_demo_plugin/lineItem.hpp index aaa9ae3..c2a3dae 100644 --- a/include/rqt_demo_plugin/lineItem.hpp +++ b/include/rqt_demo_plugin/lineItem.hpp @@ -1,45 +1,35 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -class LineItem : public QQuickItem -{ +#include + +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); + public: + LineItem(QQuickItem* parent = nullptr); ~LineItem(); - QSGNode *updatePaintNode(QSGNode *, UpdatePaintNodeData *) override; + QSGNode* updatePaintNode(QSGNode*, UpdatePaintNodeData*) override; - QString stupid() const {return stupid_;} - QObject* handler() const {return handler_;} + QString stupid() const { return stupid_; } + QObject* handler() const { return handler_; } - void setStupid(const QString &str) {stupid_ = str;} - void setScale(const float scale) {scale_ = scale;} - void setHandler(QObject* const obj) {handler_ = obj;} + void setStupid(const QString& str) { stupid_ = str; } + void setScale(const float scale) { scale_ = scale; } + void setHandler(QObject* const obj); -signals: - void stupidChanged(const QString &str); + signals: + void stupidChanged(const QString& str); void handlerChanged(QObject* const obj); - - -private: - float scale_; - QString stupid_; + private: + float scale_; + QString stupid_; QObject* handler_; - int prev_size=0; + int prev_size = 0; }; \ No newline at end of file diff --git a/include/rqt_demo_plugin/rqt_demo_plugin.hpp b/include/rqt_demo_plugin/rqt_demo_plugin.hpp index edc4b3d..cc517c8 100644 --- a/include/rqt_demo_plugin/rqt_demo_plugin.hpp +++ b/include/rqt_demo_plugin/rqt_demo_plugin.hpp @@ -1,70 +1,112 @@ #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 #include +#include + #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include inline void initResources() { Q_INIT_RESOURCE(res); } inline void cleanupResources() { Q_CLEANUP_RESOURCE(res); } namespace mg { -class DemoPluginMg : public rqt_gui_cpp::Plugin { - Q_OBJECT - Q_PROPERTY(QObject* demoPlugin READ demoPlugin NOTIFY demoPluginChanged) -public: - DemoPluginMg(); + 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) - virtual void shutdownPlugin() { - cleanupResources(); - } + public: + std::mutex tree_mut_; + bool children_dirty; + bool points_dirty; - virtual void initPlugin(qt_gui_cpp::PluginContext &); + DemoPluginMg(); - QObject * demoPlugin() const { - return (QObject *)(this); - } + void initPlugin(qt_gui_cpp::PluginContext&) override; + void shutdownPlugin() override { cleanupResources(); } - const QSGGeometry::Point2D hello[8] = { - {0.2,0.2}, {0.8,0.2}, - {0.2,0.2}, {0.2,0.8}, - {0.2,0.4}, {0.4,0.8}, - {0.2,0.4}, {0.1,0.1}, - }; + Q_INVOKABLE void postPoint(qreal x, qreal y); - const int hello_c = 8; + QObject* demoPlugin() const { return (QObject*)(this); } + QPointF robotPos() const { return robotPos_; } + float robotTheta() const { return robotTheta_; } -signals: + const std::vector>& children() const { return children_; } + const std::vector>& points() const { return points_; } - void demoPluginChanged(QObject * const); + int cchildren() const { return cchildren_; }; -}; + signals: -inline void DemoPluginMg::initPlugin(qt_gui_cpp::PluginContext &pc) { - initResources(); - const QUrl qrl("qrc:/qml/helloworld.qml"); - QQuickView *qv = new QQuickView(); - qv->rootContext()->setContextProperty("DemoPlug", this); - qv->setSource(qrl); - qv->setResizeMode(QQuickView::ResizeMode::SizeRootObjectToView); - QSurfaceFormat format = qv->format(); - format.setSamples(4); - qv->setFormat(format); - QWidget *qw_ = QWidget::createWindowContainer(qv); - pc.addWidget(qw_); -} + void demoPluginChanged(QObject* const); + void robotPosChanged(QPointF const); + void robotThetaChanged(float const); + void treeChanged(); + + private: + std::shared_ptr> editcycle_sub_; + + std::vector> children_; + std::vector> points_; + + std::shared_ptr tf_timer_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Publisher::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(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + tf_timer_ = node_->create_timer(std::chrono::milliseconds(15), [this]() { this->cb_tf_timer(); }); + pub_point_ = node_->create_publisher("/clicked_point", rclcpp::QoS(1)); + + QQuickStyle::setStyle("fusion"); + + const QUrl qrl("qrc:/qml/helloworld.qml"); + auto* qv = new QQuickView(); + + auto editcycle_cb = std::bind(&DemoPluginMg::cb_GraphEdits, this, std::placeholders::_1); + editcycle_sub_ = node_->create_subscription( + "GraphEdits", rclcpp::QoS(1000).transient_local(), editcycle_cb); + + qv->rootContext()->setContextProperty("DemoPlug", this); + qv->setSource(qrl); + qv->setResizeMode(QQuickView::ResizeMode::SizeRootObjectToView); + QSurfaceFormat format = qv->format(); + format.setSamples(4); + qv->setFormat(format); + QWidget* qw_ = QWidget::createWindowContainer(qv); + pc.addWidget(qw_); + } }; // namespace mg diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt new file mode 100644 index 0000000..df9a589 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/mg_msgs/action/LookAt.action b/mg_msgs/action/LookAt.action new file mode 100644 index 0000000..3537aef --- /dev/null +++ b/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 \ No newline at end of file diff --git a/mg_msgs/action/MovePoint.action b/mg_msgs/action/MovePoint.action new file mode 100644 index 0000000..9912d7b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/mg_msgs/action/MoveStraight.action b/mg_msgs/action/MoveStraight.action new file mode 100644 index 0000000..1921291 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/mg_msgs/action/Rotate.action b/mg_msgs/action/Rotate.action new file mode 100644 index 0000000..3948fa3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/mg_msgs/msg/Point2d.msg b/mg_msgs/msg/Point2d.msg new file mode 100644 index 0000000..125c004 --- /dev/null +++ b/mg_msgs/msg/Point2d.msg @@ -0,0 +1,2 @@ +float64 x +float64 y \ No newline at end of file diff --git a/mg_msgs/msg/TEdit.msg b/mg_msgs/msg/TEdit.msg new file mode 100644 index 0000000..4ef056d --- /dev/null +++ b/mg_msgs/msg/TEdit.msg @@ -0,0 +1,4 @@ + +uint8 type +uint16 idx +uint16 idy diff --git a/mg_msgs/msg/TEditCycle.msg b/mg_msgs/msg/TEditCycle.msg new file mode 100644 index 0000000..e9af1cc --- /dev/null +++ b/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 \ No newline at end of file diff --git a/mg_msgs/package.xml b/mg_msgs/package.xml new file mode 100644 index 0000000..b396373 --- /dev/null +++ b/mg_msgs/package.xml @@ -0,0 +1,19 @@ + + + + mg_msgs + 0.0.0 + This contains various msg/action used within the project + Pimpest + MIT + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/mg_msgs/srv/SetWidth.srv b/mg_msgs/srv/SetWidth.srv new file mode 100644 index 0000000..7e44f9a --- /dev/null +++ b/mg_msgs/srv/SetWidth.srv @@ -0,0 +1,2 @@ +float64 width +--- \ No newline at end of file diff --git a/package.xml b/package.xml index 2049575..763cfcc 100644 --- a/package.xml +++ b/package.xml @@ -14,6 +14,10 @@ rqt rqt_gui_cpp rqt_image_view + tf2 + tf2_ros + + mg_msgs ament_cmake diff --git a/resources/playmat_grid_v1.svg b/resources/playmat_grid_v1.svg new file mode 100644 index 0000000..53b2b1e --- /dev/null +++ b/resources/playmat_grid_v1.svg @@ -0,0 +1,27739 @@ + +image/svg+xml0°90°-90°180°45°30°60°120°135°150°-45°-135°ππ/2-π/20°90°-90°180°45°30°60°120°135°150°-45°-135°ππ/2-π/20°90°-90°180°45°30°60°120°135°150°-45°-135°ππ/2-π/20°90°-90°180°45°30°60°120°135°150°-45°-135°ππ/2-π/210020030040050060070080090010001100120013001400150016001700180019002000210022002300240025002600270028002900x10020030040050060070080090010001100120013001400150016001700180019002000210022002300240025002600270028002900190018001700160015001400130012001100100090080070060050040030020010019001800170016001500140013001200110010009008007006005004003002001000yyy190018001700160015001400130012001100100090080070060050040030020010010020030040050060070080090010001100120013001400150016001700180019002000210022002300240025002600270028002900123456DCBADCBADCBA123456123456yxxx diff --git a/resources/qml/helloworld.qml b/resources/qml/helloworld.qml index 075739e..2987d83 100644 --- a/resources/qml/helloworld.qml +++ b/resources/qml/helloworld.qml @@ -1,42 +1,116 @@ import QtQuick 2.3 +import QtQuick.Controls 2.0 import LineItem 1.0 import DemoPlugin 1.0 -Rectangle { - id: rr - color: "gray" - property var scale: 0.4 - Flickable { - width: parent.width; - height: parent.height - contentWidth: 3000*rr.scale; contentHeight: 2000*rr.scale - flickDeceleration: 10000 - leftMargin: Math.max((width - contentWidth)/2,0) - topMargin: Math.max((height - contentHeight)/2,0) - clip: true +Item { +id: root +Column { + Row { + id: topbar + Button { + id: targetMode + text: "Target" + flat: false + checkable: true + onClicked: function() {} + } + } - Rectangle { - color: "white" - id: contentRect - anchors.top: parent.top - anchors.left: parent.left - width: parent.width/rr.scale - height: parent.height/rr.scale + Rectangle { + height: root.height - topbar.height + width: root.width + id: rr + color: "dimgrey" + property var scale: 0.5 + + Flickable { + id: viewport + width: parent.width; + height: parent.height + contentWidth: 3000; contentHeight: 2000 + flickDeceleration: 10000 + leftMargin: Math.max((width - contentWidth)/2,0) + topMargin: Math.max((height - contentHeight)/2,0) - transform: Scale {yScale: rr.scale; xScale: rr.scale} - transformOrigin: Item.TopLeft + clip: true - Row { - x:0 - y:0 - - LineItem { + Rectangle { + color: "lightgrey" + id: contentRect + anchors.top: parent.top + anchors.left: parent.left + width: parent.width/rr.scale + height: parent.height/rr.scale + + transform: Scale {yScale: rr.scale; xScale: rr.scale} + transformOrigin: Item.TopLeft + Image { + x: 0;y: 0 height: 2000 width: 3000 - scale: rr.scale - handler: DemoPlug.demoPlugin + source: "../playmat_grid_v1.svg" + antialiasing: false + } + + Row { + x:0 + y:0 + + LineItem { + height: 2000 + width: 3000 + scale: rr.scale + 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 + } + + + } + } +} } \ No newline at end of file diff --git a/resources/res.qrc b/resources/res.qrc index 3b1d222..5e98866 100644 --- a/resources/res.qrc +++ b/resources/res.qrc @@ -1,5 +1,7 @@ qml/helloworld.qml + robot.jpg + playmat_grid_v1.svg \ No newline at end of file diff --git a/resources/robot.jpg b/resources/robot.jpg new file mode 100644 index 0000000..da588d8 Binary files /dev/null and b/resources/robot.jpg differ diff --git a/src/lineItem.cpp b/src/lineItem.cpp index 8613722..b63a4ac 100644 --- a/src/lineItem.cpp +++ b/src/lineItem.cpp @@ -1,71 +1,99 @@ #include "rqt_demo_plugin/lineItem.hpp" #include "rqt_demo_plugin/rqt_demo_plugin.hpp" +#include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include -LineItem::LineItem(QQuickItem *parent) - : QQuickItem(parent) - , scale_(1) - , stupid_("") - , handler_(nullptr) -{ +LineItem::LineItem(QQuickItem* parent) : 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(handler_); + connect(plugin, SIGNAL(treeChanged()), this, SLOT(update())); + emit handlerChanged(obj); + update(); +} -QSGNode *LineItem::updatePaintNode(QSGNode *old, UpdatePaintNodeData *) -{ - QSGGeometryNode *node = nullptr; - QSGGeometry *geometry = nullptr; - mg::DemoPluginMg *plugin = static_cast(handler_); - if(!plugin) { +QSGNode* LineItem::updatePaintNode(QSGNode* old, UpdatePaintNodeData*) { + QSGGeometryNode* node = nullptr; + QSGGeometry* geometry = nullptr; + auto* plugin = dynamic_cast(handler_); + + if (!plugin) { qmlEngine(this)->throwError("Handler plugin was not provided"); return old; } - - const int hello_c = plugin->hello_c; - - if(!old) { - 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; - material->setColor(QColor(255,0,0)); - node->setMaterial(material); - node->setFlag(QSGNode::OwnsMaterial); - - } else { - node = static_cast(old); - geometry = node->geometry(); - if(hello_c != prev_size) { - geometry->allocate(prev_size); - } + if (plugin->cchildren() == 0) { + return nullptr; } + std::lock_guard lock(plugin->tree_mut_); - if(hello_c != prev_size) { + if (!old) { + geometry + = new QSGGeometry(QSGGeometry::defaultAttributes_Point2D(), plugin->points().size(), 2 * plugin->cchildren()); + + node = new QSGGeometryNode; + 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 = dynamic_cast(old); + geometry = node->geometry(); + + geometry->allocate(plugin->points().size(), 2 * plugin->cchildren()); + } + + if (plugin->points_dirty || plugin->children_dirty) { const QSizeF s = size(); - QSGGeometry::Point2D *verticies = geometry->vertexDataAsPoint2D(); - for(int i = 0; ihello[i].x * s.width(); - verticies[i].y = plugin->hello[i].y * s.height(); + + const std::vector>& points = plugin->points(); + QSGGeometry::Point2D* verticies = geometry->vertexDataAsPoint2D(); + + int i = 0; + 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); - prev_size = hello_c; + plugin->points_dirty = false; } - - return node; + + if (plugin->children_dirty) { + const std::vector>& 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; } \ No newline at end of file diff --git a/src/rqt_demo_plugin.cpp b/src/rqt_demo_plugin.cpp index b0dcdbd..be320f0 100644 --- a/src/rqt_demo_plugin.cpp +++ b/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 #include #include #include + PLUGINLIB_EXPORT_CLASS(mg::DemoPluginMg, rqt_gui_cpp::Plugin) -mg::DemoPluginMg::DemoPluginMg() : Plugin() { - QSurfaceFormat fmt; - fmt.setSwapInterval(0); - fmt.setRenderableType(QSurfaceFormat::OpenGLES); - QSurfaceFormat::setDefaultFormat(fmt); - static bool typesRegistered = 0; - if(!typesRegistered) { - qmlRegisterType("LineItem", 1, 0, "LineItem"); - qmlRegisterAnonymousType("DemoPlugin", 1); - typesRegistered = 1; - } +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 = false; + if (!typesRegistered) { + qmlRegisterType("LineItem", 1, 0, "LineItem"); + qmlRegisterAnonymousType("DemoPlugin", 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); } \ No newline at end of file