From 78f2405075809200228b7866bb8df22cc642d1ae Mon Sep 17 00:00:00 2001 From: dhood Date: Thu, 4 Jan 2018 23:21:43 -0800 Subject: [PATCH] Normalize quaternions --- .../scripts/basic_controls.py | 20 ++++++ .../src/basic_controls.cpp | 63 ++++++++----------- interactive_marker_tutorials/src/pong.cpp | 12 ++-- .../src/selection.cpp | 21 ++++--- 4 files changed, 66 insertions(+), 50 deletions(-) diff --git a/interactive_marker_tutorials/scripts/basic_controls.py b/interactive_marker_tutorials/scripts/basic_controls.py index e57ed621..cf4686eb 100755 --- 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 index 45472f67..61c0a6a2 100644 --- a/interactive_marker_tutorials/src/basic_controls.cpp +++ b/interactive_marker_tutorials/src/basic_controls.cpp @@ -226,10 +226,9 @@ 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); @@ -237,10 +236,9 @@ void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector 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); @@ -248,10 +246,9 @@ void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector 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 index d6a2d3c6..f958fab1 100644 --- a/interactive_marker_tutorials/src/pong.cpp +++ b/interactive_marker_tutorials/src/pong.cpp @@ -34,6 +34,8 @@ #include #include +#include + 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 index 8cc5a0c9..fc8a78d2 100644 --- a/interactive_marker_tutorials/src/selection.cpp +++ b/interactive_marker_tutorials/src/selection.cpp @@ -36,6 +36,7 @@ #include #include +#include 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; }