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

128
src/lineItem.cpp
View File

@ -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 <qquickitem.h>
#include <qsgflatcolormaterial.h>
#include <qsggeometry.h>
#include <qsgnode.h>
#include <qsize.h>
#include <qobjectdefs.h>
#include <sys/types.h>
#include <unordered_set>
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<mg::DemoPluginMg*>(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<mg::DemoPluginMg *>(handler_);
if(!plugin) {
QSGNode* LineItem::updatePaintNode(QSGNode* old, UpdatePaintNodeData*) {
QSGGeometryNode* node = nullptr;
QSGGeometry* geometry = nullptr;
auto* plugin = dynamic_cast<mg::DemoPluginMg*>(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<QSGGeometryNode *>(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<QSGGeometryNode*>(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; i<hello_c; i++) {
verticies[i].x = plugin->hello[i].x * s.width();
verticies[i].y = plugin->hello[i].y * s.height();
const std::vector<std::pair<float, float>>& 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<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 "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() {
QSurfaceFormat fmt;
fmt.setSwapInterval(0);
fmt.setRenderableType(QSurfaceFormat::OpenGLES);
QSurfaceFormat::setDefaultFormat(fmt);
static bool typesRegistered = 0;
if(!typesRegistered) {
qmlRegisterType<LineItem>("LineItem", 1, 0, "LineItem");
qmlRegisterAnonymousType<DemoPluginMg>("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>("LineItem", 1, 0, "LineItem");
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);
}