Normalize quaternions (#40)

ros-visualization · Jan 5, 2018 · 7f1867e · 7f1867e
1 parent 4607520
commit 7f1867e
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 50 deletions.
diff --git a/interactive_marker_tutorials/scripts/basic_controls.py b/interactive_marker_tutorials/scripts/basic_controls.py
@@ -131,6 +131,14 @@ def saveMarker( int_marker ):
 #####################################################################
 # Marker Creation
 
+def normalizeQuaternion( quaternion_msg ):
+    norm = quaternion_msg.x**2 + quaternion_msg.y**2 + quaternion_msg.z**2 + quaternion_msg.w**2
+    s = norm**(-0.5)
+    quaternion_msg.x *= s
+    quaternion_msg.y *= s
+    quaternion_msg.z *= s
+    quaternion_msg.w *= s
+
 def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
     int_marker = InteractiveMarker()
     int_marker.header.frame_id = "base_link"
@@ -165,6 +173,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
         control.orientation.x = 1
         control.orientation.y = 0
         control.orientation.z = 0
+        normalizeQuaternion(control.orientation)
         control.name = "rotate_x"
         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
         if fixed:
@@ -176,6 +185,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
         control.orientation.x = 1
         control.orientation.y = 0
         control.orientation.z = 0
+        normalizeQuaternion(control.orientation)
         control.name = "move_x"
         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
         if fixed:
@@ -187,6 +197,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
         control.orientation.x = 0
         control.orientation.y = 1
         control.orientation.z = 0
+        normalizeQuaternion(control.orientation)
         control.name = "rotate_z"
         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
         if fixed:
@@ -198,6 +209,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
         control.orientation.x = 0
         control.orientation.y = 1
         control.orientation.z = 0
+        normalizeQuaternion(control.orientation)
         control.name = "move_z"
         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
         if fixed:
@@ -209,6 +221,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
         control.orientation.x = 0
         control.orientation.y = 0
         control.orientation.z = 1
+        normalizeQuaternion(control.orientation)
         control.name = "rotate_y"
         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
         if fixed:
@@ -220,6 +233,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
         control.orientation.x = 0
         control.orientation.y = 0
         control.orientation.z = 1
+        normalizeQuaternion(control.orientation)
         control.name = "move_y"
         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
         if fixed:
@@ -247,6 +261,7 @@ def makeRandomDofMarker( position ):
         control.orientation.x = rand(-1,1)
         control.orientation.y = rand(-1,1)
         control.orientation.z = rand(-1,1)
+        normalizeQuaternion(control.orientation)
         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
         int_marker.controls.append(copy.deepcopy(control))
         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
@@ -300,6 +315,7 @@ def makeQuadrocopterMarker(position):
     control.orientation.x = 0
     control.orientation.y = 1
     control.orientation.z = 0
+    normalizeQuaternion(control.orientation)
     control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
     int_marker.controls.append(copy.deepcopy(control))
     control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
@@ -321,6 +337,7 @@ def makeChessPieceMarker(position):
     control.orientation.x = 0
     control.orientation.y = 1
     control.orientation.z = 0
+    normalizeQuaternion(control.orientation)
     control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
     int_marker.controls.append(copy.deepcopy(control))
 
@@ -351,6 +368,7 @@ def makePanTiltMarker(position):
     control.orientation.x = 0
     control.orientation.y = 1
     control.orientation.z = 0
+    normalizeQuaternion(control.orientation)
     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
     control.orientation_mode = InteractiveMarkerControl.FIXED
     int_marker.controls.append(control)
@@ -360,6 +378,7 @@ def makePanTiltMarker(position):
     control.orientation.x = 0
     control.orientation.y = 0
     control.orientation.z = 1
+    normalizeQuaternion(control.orientation)
     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
     control.orientation_mode = InteractiveMarkerControl.INHERIT
     int_marker.controls.append(control)
@@ -405,6 +424,7 @@ def makeMovingMarker(position):
     control.orientation.x = 1
     control.orientation.y = 0
     control.orientation.z = 0
+    normalizeQuaternion(control.orientation)
     control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
     int_marker.controls.append(copy.deepcopy(control))
 

diff --git a/interactive_marker_tutorials/src/basic_controls.cpp b/interactive_marker_tutorials/src/basic_controls.cpp
@@ -226,32 +226,29 @@ void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector
 
   if(show_6dof)
   {
-    control.orientation.w = 1;
-    control.orientation.x = 1;
-    control.orientation.y = 0;
-    control.orientation.z = 0;
+    tf::Quaternion orien(1.0, 0.0, 0.0, 1.0);
+    orien.normalize();
+    tf::quaternionTFToMsg(orien, control.orientation);
     control.name = "rotate_x";
     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
     int_marker.controls.push_back(control);
     control.name = "move_x";
     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
     int_marker.controls.push_back(control);
 
-    control.orientation.w = 1;
-    control.orientation.x = 0;
-    control.orientation.y = 1;
-    control.orientation.z = 0;
+    orien = tf::Quaternion(0.0, 1.0, 0.0, 1.0);
+    orien.normalize();
+    tf::quaternionTFToMsg(orien, control.orientation);
     control.name = "rotate_z";
     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
     int_marker.controls.push_back(control);
     control.name = "move_z";
     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
     int_marker.controls.push_back(control);
 
-    control.orientation.w = 1;
-    control.orientation.x = 0;
-    control.orientation.y = 0;
-    control.orientation.z = 1;
+    orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
+    orien.normalize();
+    tf::quaternionTFToMsg(orien, control.orientation);
     control.name = "rotate_y";
     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
     int_marker.controls.push_back(control);
@@ -284,10 +281,9 @@ void makeRandomDofMarker( const tf::Vector3& position )
 
   for ( int i=0; i<3; i++ )
   {
-    control.orientation.w = rand(-1,1);
-    control.orientation.x = rand(-1,1);
-    control.orientation.y = rand(-1,1);
-    control.orientation.z = rand(-1,1);
+    tf::Quaternion orien(rand(-1,1), rand(-1,1), rand(-1,1), rand(-1,1));
+    orien.normalize();
+    tf::quaternionTFToMsg(orien, control.orientation);
     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
     int_marker.controls.push_back(control);
     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
@@ -354,10 +350,9 @@ void makeQuadrocopterMarker( const tf::Vector3& position )
 
   InteractiveMarkerControl control;
 
-  control.orientation.w = 1;
-  control.orientation.x = 0;
-  control.orientation.y = 1;
-  control.orientation.z = 0;
+  tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
+  orien.normalize();
+  tf::quaternionTFToMsg(orien, control.orientation);
   control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
   int_marker.controls.push_back(control);
   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
@@ -381,10 +376,9 @@ void makeChessPieceMarker( const tf::Vector3& position )
 
   InteractiveMarkerControl control;
 
-  control.orientation.w = 1;
-  control.orientation.x = 0;
-  control.orientation.y = 1;
-  control.orientation.z = 0;
+  tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
+  orien.normalize();
+  tf::quaternionTFToMsg(orien, control.orientation);
   control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
   int_marker.controls.push_back(control);
 
@@ -417,18 +411,16 @@ void makePanTiltMarker( const tf::Vector3& position )
 
   InteractiveMarkerControl control;
 
-  control.orientation.w = 1;
-  control.orientation.x = 0;
-  control.orientation.y = 1;
-  control.orientation.z = 0;
+  tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
+  orien.normalize();
+  tf::quaternionTFToMsg(orien, control.orientation);
   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
   control.orientation_mode = InteractiveMarkerControl::FIXED;
   int_marker.controls.push_back(control);
 
-  control.orientation.w = 1;
-  control.orientation.x = 0;
-  control.orientation.y = 0;
-  control.orientation.z = 1;
+  orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
+  orien.normalize();
+  tf::quaternionTFToMsg(orien, control.orientation);
   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
   control.orientation_mode = InteractiveMarkerControl::INHERIT;
   int_marker.controls.push_back(control);
@@ -504,10 +496,9 @@ void makeMovingMarker( const tf::Vector3& position )
 
   InteractiveMarkerControl control;
 
-  control.orientation.w = 1;
-  control.orientation.x = 1;
-  control.orientation.y = 0;
-  control.orientation.z = 0;
+  tf::Quaternion orien(1.0, 0.0, 0.0, 1.0);
+  orien.normalize();
+  tf::quaternionTFToMsg(orien, control.orientation);
   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
   int_marker.controls.push_back(control);
 

diff --git a/interactive_marker_tutorials/src/pong.cpp b/interactive_marker_tutorials/src/pong.cpp
@@ -34,6 +34,8 @@
 #include <math.h>
 #include <boost/thread/mutex.hpp>
 
+#include <tf/tf.h>
+
 using namespace visualization_msgs;
 
 static const float FIELD_WIDTH = 12.0;
@@ -361,8 +363,9 @@ class PongGame
     InteractiveMarkerControl control;
     control.always_visible = false;
     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
-    control.orientation.w = 1;
-    control.orientation.z = 1;
+    tf::Quaternion orien(0.0, 0.0, 1.0, 1.0);
+    orien.normalize();
+    tf::quaternionTFToMsg(orien, control.orientation);
 
     // Add a visualization marker
     Marker marker;
@@ -442,8 +445,9 @@ class PongGame
     int_marker.name = "ball";
 
     control.interaction_mode = InteractiveMarkerControl::NONE;
-    control.orientation.w = 1;
-    control.orientation.y = 1;
+    tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
+    orien.normalize();
+    tf::quaternionTFToMsg(orien, control.orientation);
 
     Marker marker;
     marker.color.r = 1.0;

diff --git a/interactive_marker_tutorials/src/selection.cpp b/interactive_marker_tutorials/src/selection.cpp
@@ -36,6 +36,7 @@
 #include <interactive_markers/tools.h>
 
 #include <tf/LinearMath/Vector3.h>
+#include <tf/tf.h>
 
 bool testPointAgainstAabb2(const tf::Vector3 &aabbMin1, const tf::Vector3 &aabbMax1,
                            const tf::Vector3 &point)
@@ -209,7 +210,7 @@ class PointCouldSelector
         control.orientation_mode = vm::InteractiveMarkerControl::INHERIT;
         control.always_visible = false;
 
-        control.orientation.w = 1;
+        tf::Quaternion orien;
 
         switch ( axis )
         {
@@ -218,27 +219,27 @@ class PointCouldSelector
           int_marker.pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x();
           int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
           int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
-          control.orientation.x = 1;
-          control.orientation.y = 0;
-          control.orientation.z = 0;
+          orien = tf::Quaternion(1.0, 0.0, 0.0, 1.0);
+          orien.normalize();
+          tf::quaternionTFToMsg(orien, control.orientation);
           break;
         case 1:
           int_marker.name = sign>0 ? "max_y" : "min_y";
           int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
           int_marker.pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y();
           int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
-          control.orientation.x = 0;
-          control.orientation.y = 0;
-          control.orientation.z = 1;
+          orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
+          orien.normalize();
+          tf::quaternionTFToMsg(orien, control.orientation);
           break;
         default:
           int_marker.name = sign>0 ? "max_z" : "min_z";
           int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
           int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
           int_marker.pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z();
-          control.orientation.x = 0;
-          control.orientation.y = -1;
-          control.orientation.z = 0;
+          orien = tf::Quaternion(0.0, -1.0, 0.0, 1.0);
+          orien.normalize();
+          tf::quaternionTFToMsg(orien, control.orientation);
           break;
         }