Pimpest/magrob-rqt-plugin
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added robot and things

Browse Source
This commit is contained in:
Pimpest
2025-03-23 13:21:23 +01:00
parent 955aa794fe
commit b41ceb51ee
23 changed files with 28380 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
.clang-format Normal file
View File

@ -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

34
.clang-tidy Normal file
View File

@ -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

6
.clangd Normal file
View File

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

14
CMakeLists.txt
View File

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

44
include/rqt_demo_plugin/lineItem.hpp
View File

@ -1,45 +1,35 @@
#pragma once #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_OBJECT
Q_PROPERTY(float scale READ scale WRITE setScale NOTIFY scaleChanged) Q_PROPERTY(float scale READ scale WRITE setScale NOTIFY scaleChanged)
Q_PROPERTY(QString stupid READ stupid WRITE setStupid NOTIFY stupidChanged) Q_PROPERTY(QString stupid READ stupid WRITE setStupid NOTIFY stupidChanged)
Q_PROPERTY(QObject* handler READ handler WRITE setHandler NOTIFY handlerChanged) Q_PROPERTY(QObject* handler READ handler WRITE setHandler NOTIFY handlerChanged)
QML_ELEMENT QML_ELEMENT
public: public:
LineItem(QQuickItem *parent = nullptr); LineItem(QQuickItem* parent = nullptr);
~LineItem(); ~LineItem();
QSGNode *updatePaintNode(QSGNode *, UpdatePaintNodeData *) override; QSGNode* updatePaintNode(QSGNode*, UpdatePaintNodeData*) override;
QString stupid() const {return stupid_;} QString stupid() const { return stupid_; }
QObject* handler() const {return handler_;} QObject* handler() const { return handler_; }
void setStupid(const QString &str) {stupid_ = str;} void setStupid(const QString& str) { stupid_ = str; }
void setScale(const float scale) {scale_ = scale;} void setScale(const float scale) { scale_ = scale; }
void setHandler(QObject* const obj) {handler_ = obj;} void setHandler(QObject* const obj);
signals: signals:
void stupidChanged(const QString &str); void stupidChanged(const QString& str);
void handlerChanged(QObject* const obj); void handlerChanged(QObject* const obj);
private:
float scale_;
private: QString stupid_;
float scale_;
QString stupid_;
QObject* handler_; QObject* handler_;
int prev_size=0; int prev_size = 0;
}; };

132
include/rqt_demo_plugin/rqt_demo_plugin.hpp
View File

@ -1,70 +1,112 @@
#pragma once #pragma once
#include "geometry_msgs/msg/point.hpp"
#include "rqt_gui_cpp/plugin.h" #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 <QtGui>
#include <QtQuick> #include <QtQuick>
#include <QtQuickControls2/QQuickStyle>
#include <QtWidgets> #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 initResources() { Q_INIT_RESOURCE(res); }
inline void cleanupResources() { Q_CLEANUP_RESOURCE(res); } inline void cleanupResources() { Q_CLEANUP_RESOURCE(res); }
namespace mg { namespace mg {
class DemoPluginMg : public rqt_gui_cpp::Plugin { class DemoPluginMg : public rqt_gui_cpp::Plugin {
Q_OBJECT Q_OBJECT
Q_PROPERTY(QObject* demoPlugin READ demoPlugin NOTIFY demoPluginChanged) Q_PROPERTY(QObject* demoPlugin READ demoPlugin NOTIFY demoPluginChanged)
public: Q_PROPERTY(QPointF robotPos READ robotPos NOTIFY robotPosChanged)
DemoPluginMg(); Q_PROPERTY(qreal robotTheta READ robotTheta NOTIFY robotThetaChanged)
virtual void shutdownPlugin() { public:
cleanupResources(); std::mutex tree_mut_;
} bool children_dirty;
bool points_dirty;
virtual void initPlugin(qt_gui_cpp::PluginContext &); DemoPluginMg();
QObject * demoPlugin() const { void initPlugin(qt_gui_cpp::PluginContext&) override;
return (QObject *)(this); void shutdownPlugin() override { cleanupResources(); }
}
const QSGGeometry::Point2D hello[8] = { Q_INVOKABLE void postPoint(qreal x, qreal y);
{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},
};
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<std::unordered_set<int>>& children() const { return children_; }
const std::vector<std::pair<float, float>>& points() const { return points_; }
void demoPluginChanged(QObject * const); int cchildren() const { return cchildren_; };
}; signals:
inline void DemoPluginMg::initPlugin(qt_gui_cpp::PluginContext &pc) { void demoPluginChanged(QObject* const);
initResources(); void robotPosChanged(QPointF const);
const QUrl qrl("qrc:/qml/helloworld.qml"); void robotThetaChanged(float const);
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 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");
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);
QSurfaceFormat format = qv->format();
format.setSamples(4);
qv->setFormat(format);
QWidget* qw_ = QWidget::createWindowContainer(qv);
pc.addWidget(qw_);
}
}; // namespace mg }; // namespace mg

23
mg_msgs/CMakeLists.txt Normal file
View File

@ -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()

25
mg_msgs/action/LookAt.action Normal file
View File

@ -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

25
mg_msgs/action/MovePoint.action Normal file
View File

@ -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

24
mg_msgs/action/MoveStraight.action Normal file
View File

@ -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

25
mg_msgs/action/Rotate.action Normal file
View File

@ -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

2
mg_msgs/msg/Point2d.msg Normal file
View File

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

4
mg_msgs/msg/TEdit.msg Normal file
View File

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

8
mg_msgs/msg/TEditCycle.msg Normal file
View File

@ -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

19
mg_msgs/package.xml Normal file
View File

@ -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>

2
mg_msgs/srv/SetWidth.srv Normal file
View File

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

4
package.xml
View File

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

27739
resources/playmat_grid_v1.svg Normal file
View File

File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 2.6 MiB

130
resources/qml/helloworld.qml
View File

@ -1,42 +1,116 @@
import QtQuick 2.3 import QtQuick 2.3
import QtQuick.Controls 2.0
import LineItem 1.0 import LineItem 1.0
import DemoPlugin 1.0 import DemoPlugin 1.0
Rectangle { Item {
id: rr id: root
color: "gray" Column {
property var scale: 0.4 Row {
Flickable { id: topbar
width: parent.width; Button {
height: parent.height id: targetMode
contentWidth: 3000*rr.scale; contentHeight: 2000*rr.scale text: "Target"
flickDeceleration: 10000 flat: false
leftMargin: Math.max((width - contentWidth)/2,0) checkable: true
topMargin: Math.max((height - contentHeight)/2,0) onClicked: function() {}
clip: true }
}
Rectangle { Rectangle {
color: "white" height: root.height - topbar.height
id: contentRect width: root.width
anchors.top: parent.top id: rr
anchors.left: parent.left color: "dimgrey"
width: parent.width/rr.scale property var scale: 0.5
height: parent.height/rr.scale
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} clip: true
transformOrigin: Item.TopLeft
Row { Rectangle {
x:0 color: "lightgrey"
y:0 id: contentRect
anchors.top: parent.top
LineItem { 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 height: 2000
width: 3000 width: 3000
scale: rr.scale source: "../playmat_grid_v1.svg"
handler: DemoPlug.demoPlugin 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
}
}
} }
}
} }

2
resources/res.qrc
View File

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

BIN
resources/robot.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 14 KiB

128
src/lineItem.cpp
View File

@ -1,71 +1,99 @@
#include "rqt_demo_plugin/lineItem.hpp" #include "rqt_demo_plugin/lineItem.hpp"
#include "rqt_demo_plugin/rqt_demo_plugin.hpp" #include "rqt_demo_plugin/rqt_demo_plugin.hpp"
#include <mutex>
#include <qglobal.h> #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); setFlag(ItemHasContents, true);
} }
LineItem::~LineItem() = default; 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;
QSGGeometryNode *node = nullptr; QSGGeometry* geometry = nullptr;
QSGGeometry *geometry = nullptr; auto* plugin = dynamic_cast<mg::DemoPluginMg*>(handler_);
mg::DemoPluginMg *plugin = static_cast<mg::DemoPluginMg *>(handler_);
if(!plugin) { if (!plugin) {
qmlEngine(this)->throwError("Handler plugin was not provided"); qmlEngine(this)->throwError("Handler plugin was not provided");
return old; return old;
} }
if (plugin->cchildren() == 0) {
const int hello_c = plugin->hello_c; return nullptr;
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<QSGGeometryNode *>(old);
geometry = node->geometry();
if(hello_c != prev_size) {
geometry->allocate(prev_size);
}
} }
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<QSGGeometryNode*>(old);
geometry = node->geometry();
geometry->allocate(plugin->points().size(), 2 * plugin->cchildren());
}
if (plugin->points_dirty || plugin->children_dirty) {
const QSizeF s = size(); const QSizeF s = size();
QSGGeometry::Point2D *verticies = geometry->vertexDataAsPoint2D();
for(int i = 0; i<hello_c; i++) { const std::vector<std::pair<float, float>>& points = plugin->points();
verticies[i].x = plugin->hello[i].x * s.width(); QSGGeometry::Point2D* verticies = geometry->vertexDataAsPoint2D();
verticies[i].y = plugin->hello[i].y * s.height();
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(); geometry->markVertexDataDirty();
node->markDirty(QSGNode::DirtyGeometry); plugin->points_dirty = false;
prev_size = hello_c;
} }
return node; 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;
} }

83
src/rqt_demo_plugin.cpp
View File

@ -1,21 +1,82 @@
#include "rqt_demo_plugin/rqt_demo_plugin.hpp" #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 "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 "pluginlib/class_list_macros.hpp"
#include <mutex>
#include <qnamespace.h> #include <qnamespace.h>
#include <qqml.h> #include <qqml.h>
#include <qsurfaceformat.h> #include <qsurfaceformat.h>
PLUGINLIB_EXPORT_CLASS(mg::DemoPluginMg, rqt_gui_cpp::Plugin) PLUGINLIB_EXPORT_CLASS(mg::DemoPluginMg, rqt_gui_cpp::Plugin)
mg::DemoPluginMg::DemoPluginMg() : Plugin() { mg::DemoPluginMg::DemoPluginMg() :
QSurfaceFormat fmt; children_dirty(false), points_dirty(false), robotPos_(0, 0), robotTheta_(0), cchildren_(0) {
fmt.setSwapInterval(0); QSurfaceFormat fmt;
fmt.setRenderableType(QSurfaceFormat::OpenGLES); fmt.setSwapInterval(0);
QSurfaceFormat::setDefaultFormat(fmt); fmt.setRenderableType(QSurfaceFormat::OpenGLES);
static bool typesRegistered = 0; QSurfaceFormat::setDefaultFormat(fmt);
if(!typesRegistered) { static bool typesRegistered = false;
qmlRegisterType<LineItem>("LineItem", 1, 0, "LineItem"); if (!typesRegistered) {
qmlRegisterAnonymousType<DemoPluginMg>("DemoPlugin", 1); qmlRegisterType<LineItem>("LineItem", 1, 0, "LineItem");
typesRegistered = 1; qmlRegisterAnonymousType<DemoPluginMg>("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);
} }