82 lines
2.7 KiB
C++
82 lines
2.7 KiB
C++
#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() :
|
|
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);
|
|
} |