interconnect with rviz

rhaschke · Oct 25, 2015 · 104c869 · 104c869
1 parent 1226dfc
commit 104c869
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 1 deletion.
diff --git a/src/RotationControl.cpp b/src/RotationControl.cpp
@@ -50,6 +50,8 @@ void RotationControl::setValue(const Eigen::Quaterniond &q) {
 	if (!q.isApprox(_ew->value())) _ew->setValue(q);
 
 	updatePose(_q);
+	_server->setPose(_title, _pose);
+	_server->applyChanges();
 
 	emit valueChanged(q);
 }
@@ -122,5 +124,11 @@ void RotationControl::createInteractiveMarker(const Eigen::Vector3d &pos,
 	ctrl.markers.push_back(createBoxMarker(3*s, 2*s, 1*s, color));
 	imarker.controls.push_back(ctrl);
 
-	_server->insert(imarker);
+	_server->insert(imarker, boost::bind(&RotationControl::processFeedback, this, _1));
+}
+
+void RotationControl::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
+{
+	const geometry_msgs::Quaternion &q = feedback->pose.orientation;
+	setValue(Eigen::Quaterniond(q.w, q.x, q.y, q.z));
 }
diff --git a/src/RotationControl.h b/src/RotationControl.h
@@ -36,6 +36,7 @@ public slots:
 	void setupUi();
 	void createInteractiveMarker(const Eigen::Vector3d &position, const QColor &color);
 	void updatePose(const Eigen::Quaterniond &q);
+	void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&);
 
 private:
 	Eigen::Quaterniond  _q;