113 lines
3.7 KiB
C++
113 lines
3.7 KiB
C++
#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 <chrono>
|
|
#include <functional>
|
|
#include <memory>
|
|
#include <rclcpp/publisher.hpp>
|
|
#include <rclcpp/qos.hpp>
|
|
#include <unordered_set>
|
|
#include <vector>
|
|
|
|
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)
|
|
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();
|
|
|
|
void initPlugin(qt_gui_cpp::PluginContext&) override;
|
|
void shutdownPlugin() override { cleanupResources(); }
|
|
|
|
Q_INVOKABLE void postPoint(qreal x, qreal y);
|
|
|
|
QObject* demoPlugin() const { return (QObject*)(this); }
|
|
QPointF robotPos() const { return robotPos_; }
|
|
float robotTheta() const { return robotTheta_; }
|
|
|
|
const std::vector<std::unordered_set<int>>& children() const { return children_; }
|
|
const std::vector<std::pair<float, float>>& points() const { return points_; }
|
|
|
|
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");
|
|
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
|