From 166a69125b160d95ceafa689619996945ef74664 Mon Sep 17 00:00:00 2001
From: Unknown <antoine.harle@etu.upmc.fr>
Date: Mon, 2 Jul 2018 11:47:26 +0200
Subject: [PATCH] Ajout de la fonction 3DOF (normal)

---
 .../rviz_interface/src/InteractiveObject.cpp  | 32 +++++++++++++++++--
 .../rviz_interface/src/InteractiveObject.hpp  |  4 ++-
 .../rviz_interface/src/RvizInterface.cpp      |  1 +
 3 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.cpp b/rviz_interface/rviz_interface/src/InteractiveObject.cpp
index 8cb777d..3b6b020 100644
--- a/rviz_interface/rviz_interface/src/InteractiveObject.cpp
+++ b/rviz_interface/rviz_interface/src/InteractiveObject.cpp
@@ -113,7 +113,7 @@ void InteractiveObject::addButtoncontrol()
 	_server->applyChanges();
 }
 
-//Ajoute des controle pour 6DOF
+//Ajoute des controle pour 6DOF (Déplacement dans l'espace)
 void InteractiveObject::add6DOFcontrol()
 {
 	//// Ajout d'interactions ////
@@ -169,7 +169,7 @@ void InteractiveObject::add6DOFcontrol()
 	_server->applyChanges();
 }
 
-//Ajoute des controle pour 3DOF
+//Ajoute des controle pour 3DOF (Déplacement dans le plan x/z)
 void InteractiveObject::add3DOFcontrol()
 {
 	//// Ajout d'interactions ////
@@ -216,6 +216,34 @@ void InteractiveObject::add3DOFcontrol()
 	_server->applyChanges();
 }
 
+//Ajoute des controle pour 3DOF (Déplacement dans le plan)
+void InteractiveObject::add3DOFcontrol(const tf::Vector3& normal)
+{
+	//// Ajout d'interactions ////
+	InteractiveMarkerControl marker_control;
+
+	//_int_marker.controls[0].name= "2D";
+
+	marker_control.orientation_mode = InteractiveMarkerControl::FIXED;
+
+	//Orientation du vecteur
+	marker_control.orientation.w = 1;
+	marker_control.orientation.x = normal.x();
+	marker_control.orientation.y = normal.y();
+	marker_control.orientation.z = normal.z();
+	//Ajout des controles associées
+	marker_control.name = "move_plane";
+	marker_control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
+	_int_marker.controls.push_back(marker_control);
+
+	// add the interactive marker to our collection &
+	// tell the server to call processFeedback() when feedback arrives for it
+	_server->insert(_int_marker);
+
+	// 'commit' changes and send to all clients
+	_server->applyChanges();
+}
+
 //Met à jour la zone d'erreur
 void InteractiveObject::setErrorArea(double error)
 {
diff --git a/rviz_interface/rviz_interface/src/InteractiveObject.hpp b/rviz_interface/rviz_interface/src/InteractiveObject.hpp
index d351967..33f3cfa 100644
--- a/rviz_interface/rviz_interface/src/InteractiveObject.hpp
+++ b/rviz_interface/rviz_interface/src/InteractiveObject.hpp
@@ -42,8 +42,10 @@ public:
 	void addButtoncontrol();
 	//Ajoute des controle pour 6DOF (Déplacement dans l'espace)
 	void add6DOFcontrol();
-	//Ajoute des controle pour 3DOF (Déplacement dans le plan)
+	//Ajoute des controle pour 3DOF (Déplacement dans le plan x/z)
 	void add3DOFcontrol();
+	//Ajoute des controle pour 3DOF (Déplacement dans le plan)
+	void add3DOFcontrol(const tf::Vector3& normal);
 
 	//Ajoute des infos visuelles (Zone d'erreur)
 	void addVisuals();
diff --git a/rviz_interface/rviz_interface/src/RvizInterface.cpp b/rviz_interface/rviz_interface/src/RvizInterface.cpp
index 4104da8..2d98292 100644
--- a/rviz_interface/rviz_interface/src/RvizInterface.cpp
+++ b/rviz_interface/rviz_interface/src/RvizInterface.cpp
@@ -13,6 +13,7 @@ RvizInterface::RvizInterface(): _server("RvizInterface")
 
 	_objects.push_back(new InteractiveObject(&_server, "Test", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0)));
 	_objects[0]->add6DOFcontrol();
+	// _objects[0]->add3DOFcontrol(tf::Vector3(0.5,0.5,0));
 
 	// _objects.push_back(new InteractiveObject(&_server, "Test 2", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::CUBE, tf::Vector3(3,3,0)));
 	// _objects[1]->add3DOFcontrol();