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

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);
}