added interactive markers

Author: rhaschke

CMakeLists.txt

@@ -4,7 +4,7 @@ project(lecture)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED roscpp)
+find_package(catkin REQUIRED roscpp interactive_markers)
 find_package(Qt5 REQUIRED COMPONENTS Widgets)
 find_package(cmake_modules REQUIRED)
 find_package(Eigen REQUIRED)

package.xml

@@ -35,6 +35,7 @@
   <!-- Use build_depend for packages required at compile time: -->
   <build_depend>cmake_modules</build_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>interactive_markers</build_depend>
 
   <!-- Use run_depend for packages you need at runtime: -->
   <run_depend>roscpp</run_depend>

src/MainWindow.cpp

@@ -2,16 +2,32 @@
 #include "RotationControl.h"
 
 #include <QVBoxLayout>
+#include <Eigen/Core>
+#include <ros/ros.h>
 
 MainWindow::MainWindow(QWidget *parent) :
-   QMainWindow(parent)
+   QMainWindow(parent), spinner(1)
 {
+	server.reset(new interactive_markers::InteractiveMarkerServer("euler","",false));
+	setupUi();
+
+	server->applyChanges();
+	spinner.start();
+}
+
+MainWindow::~MainWindow()
+{
+	spinner.stop();
+}
+
+void MainWindow::setupUi() {
 	QWidget *central = new QWidget(this);
 
-	frame1 = new RotationControl(this);
-	frame2 = new RotationControl(this);
-	frame1p2 = new RotationControl(this);
-	frame1c2 = new RotationControl(this);
+	double s = 0.5;
+	frame1 = new RotationControl("frame 1", Eigen::Vector3d(-s,s,0), server, this);
+	frame2 = new RotationControl("frame 2", Eigen::Vector3d( s,s,0), server, this);
+	frame1p2 = new RotationControl("frame 1+2", Eigen::Vector3d(-s,-s,0), server, this);
+	frame1c2 = new RotationControl("frame 1*2", Eigen::Vector3d( s,-s,0), server, this);
 
 	QVBoxLayout *layout = new QVBoxLayout(central);
 	layout->addWidget(frame1);

src/MainWindow.h

@@ -1,19 +1,28 @@
 #pragma once
 
 #include <QMainWindow>
+#include <interactive_markers/interactive_marker_server.h>
+
 class RotationControl;
 
 class MainWindow : public QMainWindow
 {
 	Q_OBJECT
 public:
 	explicit MainWindow(QWidget *parent = 0);
+	~MainWindow();
 
 signals:
 
 public slots:
 
-public:
+private:
+	void setupUi();
+
+private:
+	boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
+	ros::AsyncSpinner spinner;
+
 	RotationControl *frame1;
 	RotationControl *frame2;
 	RotationControl *frame1p2;

src/RotationControl.cpp

@@ -3,14 +3,24 @@
 #include "EulerWidget.h"
 
 #include <QBoxLayout>
+#include <visualization_msgs/InteractiveMarker.h>
+#include <visualization_msgs/InteractiveMarkerFeedback.h>
 
-RotationControl::RotationControl(QWidget *parent, const QString &title) :
-   QWidget(parent)
+RotationControl::RotationControl(const std::string &title,
+                                 const Eigen::Vector3d &position,
+                                 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> &server,
+                                 QWidget *parent) :
+   QGroupBox(QString::fromStdString(title), parent), _server(server), _title(title)
 {
 	qRegisterMetaType<Eigen::Quaterniond>("Eigen::Quaterniond");
 
-	_qw = new QuaternionWidget(parent);
-	_ew = new EulerWidget(parent);
+	setupUi();
+	createInteractiveMarker(position);
+}
+
+void RotationControl::setupUi() {
+	_qw = new QuaternionWidget(this);
+	_ew = new EulerWidget(this);
 
 	QBoxLayout *layout = new QBoxLayout(QBoxLayout::TopToBottom, this);
 	layout->addWidget(_qw);
@@ -38,9 +48,13 @@ void RotationControl::setValue(const Eigen::Quaterniond &q) {
 
 	if (!q.isApprox(_qw->value())) _qw->setValue(q);
 	if (!q.isApprox(_ew->value())) _ew->setValue(q);
+
+	updatePose(_q);
+
 	emit valueChanged(q);
 }
 
+
 void RotationControl::setEulerAxes(uint a1, uint a2, uint a3) {
 	_ew->setEulerAxes(a1, a2, a3);
 }
@@ -54,3 +68,58 @@ const Eigen::Vector3d RotationControl::eulerAngles() const {
 void RotationControl::setEulerAngles(double e1, double e2, double e3) {
 	_ew->setEulerAngles(e1, e2, e3);
 }
+
+void RotationControl::updatePose(const Eigen::Quaterniond &q) {
+	_pose.orientation.w = q.w();
+	_pose.orientation.x = q.x();
+	_pose.orientation.y = q.y();
+	_pose.orientation.z = q.z();
+}
+
+static visualization_msgs::InteractiveMarkerControl createViewPlaneControl() {
+	visualization_msgs::InteractiveMarkerControl control;
+	control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
+	control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_3D;
+	control.independent_marker_orientation = true;
+	control.always_visible = true;
+	control.name = "rotate";
+
+	return control;
+}
+
+static visualization_msgs::Marker createBoxMarker(double x, double y, double z,
+                                                  const QColor &color) {
+	visualization_msgs::Marker marker;
+
+	marker.type = visualization_msgs::Marker::CUBE;
+	marker.scale.x = x;
+	marker.scale.y = y;
+	marker.scale.z = z;
+
+	marker.color.r = color.redF();
+	marker.color.g = color.greenF();
+	marker.color.b = color.blueF();
+	marker.color.a = color.alphaF();
+
+	return marker;
+}
+
+void RotationControl::createInteractiveMarker(const Eigen::Vector3d &pos) {
+	_pose.position.x = pos[0];
+	_pose.position.y = pos[1];
+	_pose.position.z = pos[2];
+	updatePose(_q);
+
+	visualization_msgs::InteractiveMarker imarker;
+	imarker.header.frame_id = "world";
+	imarker.header.stamp = ros::Time::now();
+	imarker.pose = _pose;
+	imarker.name = _title;
+	float s = imarker.scale = 0.2;
+
+	visualization_msgs::InteractiveMarkerControl ctrl = createViewPlaneControl();
+	ctrl.markers.push_back(createBoxMarker(3*s, 2*s, 1*s, QColor("grey")));
+	imarker.controls.push_back(ctrl);
+
+	_server->insert(imarker);
+}

src/RotationControl.h

@@ -1,17 +1,21 @@
 #pragma once
 
-#include <QWidget>
+#include <QGroupBox>
 #include <Eigen/Geometry>
+#include <interactive_markers/interactive_marker_server.h>
+#include <visualization_msgs/InteractiveMarkerFeedback.h>
 
 class QuaternionWidget;
 class EulerWidget;
 
-class RotationControl : public QWidget
+class RotationControl : public QGroupBox
 {
 	Q_OBJECT
 public:
-	explicit RotationControl(QWidget *parent = 0,
-	                         const QString &title="");
+	explicit RotationControl(const std::string &_title,
+	                         const Eigen::Vector3d &position,
+	                         boost::shared_ptr<interactive_markers::InteractiveMarkerServer> &_server,
+	                         QWidget *parent = 0);
 
 	const Eigen::Quaterniond& value() const;
 	const Eigen::Vector3d eulerAngles() const;
@@ -27,8 +31,16 @@ public slots:
 	void setEulerAngles(double e1, double e2, double e3);
 	void setEulerAxes(uint a1, uint a2, uint a3);
 
-public:
-	Eigen::Quaterniond _q;
+private:
+	void setupUi();
+	void createInteractiveMarker(const Eigen::Vector3d &position);
+	void updatePose(const Eigen::Quaterniond &q);
+
+private:
+	Eigen::Quaterniond  _q;
+	boost::shared_ptr<interactive_markers::InteractiveMarkerServer> _server;
+	std::string         _title;
+	geometry_msgs::Pose _pose;
 
 	QuaternionWidget *_qw;
 	EulerWidget      *_ew;