Skip to content

Commit 7f1867e

Browse files
dhooddhood
authored
Normalize quaternions (#40)
1 parent 4607520 commit 7f1867e

File tree

4 files changed

+66
-50
lines changed

4 files changed

+66
-50
lines changed

‎interactive_marker_tutorials/scripts/basic_controls.py

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,14 @@ def saveMarker( int_marker ):
131131
#####################################################################
132132
# Marker Creation
133133

134+
def normalizeQuaternion( quaternion_msg ):
135+
norm = quaternion_msg.x**2 + quaternion_msg.y**2 + quaternion_msg.z**2 + quaternion_msg.w**2
136+
s = norm**(-0.5)
137+
quaternion_msg.x *= s
138+
quaternion_msg.y *= s
139+
quaternion_msg.z *= s
140+
quaternion_msg.w *= s
141+
134142
def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
135143
int_marker = InteractiveMarker()
136144
int_marker.header.frame_id = "base_link"
@@ -165,6 +173,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
165173
control.orientation.x = 1
166174
control.orientation.y = 0
167175
control.orientation.z = 0
176+
normalizeQuaternion(control.orientation)
168177
control.name = "rotate_x"
169178
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
170179
if fixed:
@@ -176,6 +185,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
176185
control.orientation.x = 1
177186
control.orientation.y = 0
178187
control.orientation.z = 0
188+
normalizeQuaternion(control.orientation)
179189
control.name = "move_x"
180190
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
181191
if fixed:
@@ -187,6 +197,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
187197
control.orientation.x = 0
188198
control.orientation.y = 1
189199
control.orientation.z = 0
200+
normalizeQuaternion(control.orientation)
190201
control.name = "rotate_z"
191202
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
192203
if fixed:
@@ -198,6 +209,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
198209
control.orientation.x = 0
199210
control.orientation.y = 1
200211
control.orientation.z = 0
212+
normalizeQuaternion(control.orientation)
201213
control.name = "move_z"
202214
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
203215
if fixed:
@@ -209,6 +221,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
209221
control.orientation.x = 0
210222
control.orientation.y = 0
211223
control.orientation.z = 1
224+
normalizeQuaternion(control.orientation)
212225
control.name = "rotate_y"
213226
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
214227
if fixed:
@@ -220,6 +233,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
220233
control.orientation.x = 0
221234
control.orientation.y = 0
222235
control.orientation.z = 1
236+
normalizeQuaternion(control.orientation)
223237
control.name = "move_y"
224238
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
225239
if fixed:
@@ -247,6 +261,7 @@ def makeRandomDofMarker( position ):
247261
control.orientation.x = rand(-1,1)
248262
control.orientation.y = rand(-1,1)
249263
control.orientation.z = rand(-1,1)
264+
normalizeQuaternion(control.orientation)
250265
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
251266
int_marker.controls.append(copy.deepcopy(control))
252267
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
@@ -300,6 +315,7 @@ def makeQuadrocopterMarker(position):
300315
control.orientation.x = 0
301316
control.orientation.y = 1
302317
control.orientation.z = 0
318+
normalizeQuaternion(control.orientation)
303319
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
304320
int_marker.controls.append(copy.deepcopy(control))
305321
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
@@ -321,6 +337,7 @@ def makeChessPieceMarker(position):
321337
control.orientation.x = 0
322338
control.orientation.y = 1
323339
control.orientation.z = 0
340+
normalizeQuaternion(control.orientation)
324341
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
325342
int_marker.controls.append(copy.deepcopy(control))
326343

@@ -351,6 +368,7 @@ def makePanTiltMarker(position):
351368
control.orientation.x = 0
352369
control.orientation.y = 1
353370
control.orientation.z = 0
371+
normalizeQuaternion(control.orientation)
354372
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
355373
control.orientation_mode = InteractiveMarkerControl.FIXED
356374
int_marker.controls.append(control)
@@ -360,6 +378,7 @@ def makePanTiltMarker(position):
360378
control.orientation.x = 0
361379
control.orientation.y = 0
362380
control.orientation.z = 1
381+
normalizeQuaternion(control.orientation)
363382
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
364383
control.orientation_mode = InteractiveMarkerControl.INHERIT
365384
int_marker.controls.append(control)
@@ -405,6 +424,7 @@ def makeMovingMarker(position):
405424
control.orientation.x = 1
406425
control.orientation.y = 0
407426
control.orientation.z = 0
427+
normalizeQuaternion(control.orientation)
408428
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
409429
int_marker.controls.append(copy.deepcopy(control))
410430

‎interactive_marker_tutorials/src/basic_controls.cpp

Lines changed: 27 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -226,32 +226,29 @@ void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector
226226

227227
if(show_6dof)
228228
{
229-
control.orientation.w = 1;
230-
control.orientation.x = 1;
231-
control.orientation.y = 0;
232-
control.orientation.z = 0;
229+
tf::Quaternion orien(1.0, 0.0, 0.0, 1.0);
230+
orien.normalize();
231+
tf::quaternionTFToMsg(orien, control.orientation);
233232
control.name = "rotate_x";
234233
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
235234
int_marker.controls.push_back(control);
236235
control.name = "move_x";
237236
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
238237
int_marker.controls.push_back(control);
239238

240-
control.orientation.w = 1;
241-
control.orientation.x = 0;
242-
control.orientation.y = 1;
243-
control.orientation.z = 0;
239+
orien = tf::Quaternion(0.0, 1.0, 0.0, 1.0);
240+
orien.normalize();
241+
tf::quaternionTFToMsg(orien, control.orientation);
244242
control.name = "rotate_z";
245243
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
246244
int_marker.controls.push_back(control);
247245
control.name = "move_z";
248246
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
249247
int_marker.controls.push_back(control);
250248

251-
control.orientation.w = 1;
252-
control.orientation.x = 0;
253-
control.orientation.y = 0;
254-
control.orientation.z = 1;
249+
orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
250+
orien.normalize();
251+
tf::quaternionTFToMsg(orien, control.orientation);
255252
control.name = "rotate_y";
256253
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
257254
int_marker.controls.push_back(control);
@@ -284,10 +281,9 @@ void makeRandomDofMarker( const tf::Vector3& position )
284281

285282
for ( int i=0; i<3; i++ )
286283
{
287-
control.orientation.w = rand(-1,1);
288-
control.orientation.x = rand(-1,1);
289-
control.orientation.y = rand(-1,1);
290-
control.orientation.z = rand(-1,1);
284+
tf::Quaternion orien(rand(-1,1), rand(-1,1), rand(-1,1), rand(-1,1));
285+
orien.normalize();
286+
tf::quaternionTFToMsg(orien, control.orientation);
291287
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
292288
int_marker.controls.push_back(control);
293289
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
@@ -354,10 +350,9 @@ void makeQuadrocopterMarker( const tf::Vector3& position )
354350

355351
InteractiveMarkerControl control;
356352

357-
control.orientation.w = 1;
358-
control.orientation.x = 0;
359-
control.orientation.y = 1;
360-
control.orientation.z = 0;
353+
tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
354+
orien.normalize();
355+
tf::quaternionTFToMsg(orien, control.orientation);
361356
control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
362357
int_marker.controls.push_back(control);
363358
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
@@ -381,10 +376,9 @@ void makeChessPieceMarker( const tf::Vector3& position )
381376

382377
InteractiveMarkerControl control;
383378

384-
control.orientation.w = 1;
385-
control.orientation.x = 0;
386-
control.orientation.y = 1;
387-
control.orientation.z = 0;
379+
tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
380+
orien.normalize();
381+
tf::quaternionTFToMsg(orien, control.orientation);
388382
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
389383
int_marker.controls.push_back(control);
390384

@@ -417,18 +411,16 @@ void makePanTiltMarker( const tf::Vector3& position )
417411

418412
InteractiveMarkerControl control;
419413

420-
control.orientation.w = 1;
421-
control.orientation.x = 0;
422-
control.orientation.y = 1;
423-
control.orientation.z = 0;
414+
tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
415+
orien.normalize();
416+
tf::quaternionTFToMsg(orien, control.orientation);
424417
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
425418
control.orientation_mode = InteractiveMarkerControl::FIXED;
426419
int_marker.controls.push_back(control);
427420

428-
control.orientation.w = 1;
429-
control.orientation.x = 0;
430-
control.orientation.y = 0;
431-
control.orientation.z = 1;
421+
orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
422+
orien.normalize();
423+
tf::quaternionTFToMsg(orien, control.orientation);
432424
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
433425
control.orientation_mode = InteractiveMarkerControl::INHERIT;
434426
int_marker.controls.push_back(control);
@@ -504,10 +496,9 @@ void makeMovingMarker( const tf::Vector3& position )
504496

505497
InteractiveMarkerControl control;
506498

507-
control.orientation.w = 1;
508-
control.orientation.x = 1;
509-
control.orientation.y = 0;
510-
control.orientation.z = 0;
499+
tf::Quaternion orien(1.0, 0.0, 0.0, 1.0);
500+
orien.normalize();
501+
tf::quaternionTFToMsg(orien, control.orientation);
511502
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
512503
int_marker.controls.push_back(control);
513504

‎interactive_marker_tutorials/src/pong.cpp

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,8 @@
3434
#include <math.h>
3535
#include <boost/thread/mutex.hpp>
3636

37+
#include <tf/tf.h>
38+
3739
using namespace visualization_msgs;
3840

3941
static const float FIELD_WIDTH = 12.0;
@@ -361,8 +363,9 @@ class PongGame
361363
InteractiveMarkerControl control;
362364
control.always_visible = false;
363365
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
364-
control.orientation.w = 1;
365-
control.orientation.z = 1;
366+
tf::Quaternion orien(0.0, 0.0, 1.0, 1.0);
367+
orien.normalize();
368+
tf::quaternionTFToMsg(orien, control.orientation);
366369

367370
// Add a visualization marker
368371
Marker marker;
@@ -442,8 +445,9 @@ class PongGame
442445
int_marker.name = "ball";
443446

444447
control.interaction_mode = InteractiveMarkerControl::NONE;
445-
control.orientation.w = 1;
446-
control.orientation.y = 1;
448+
tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
449+
orien.normalize();
450+
tf::quaternionTFToMsg(orien, control.orientation);
447451

448452
Marker marker;
449453
marker.color.r = 1.0;

‎interactive_marker_tutorials/src/selection.cpp

Lines changed: 11 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
#include <interactive_markers/tools.h>
3737

3838
#include <tf/LinearMath/Vector3.h>
39+
#include <tf/tf.h>
3940

4041
bool testPointAgainstAabb2(const tf::Vector3 &aabbMin1, const tf::Vector3 &aabbMax1,
4142
const tf::Vector3 &point)
@@ -209,7 +210,7 @@ class PointCouldSelector
209210
control.orientation_mode = vm::InteractiveMarkerControl::INHERIT;
210211
control.always_visible = false;
211212

212-
control.orientation.w = 1;
213+
tf::Quaternion orien;
213214

214215
switch ( axis )
215216
{
@@ -218,27 +219,27 @@ class PointCouldSelector
218219
int_marker.pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x();
219220
int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
220221
int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
221-
control.orientation.x = 1;
222-
control.orientation.y = 0;
223-
control.orientation.z = 0;
222+
orien = tf::Quaternion(1.0, 0.0, 0.0, 1.0);
223+
orien.normalize();
224+
tf::quaternionTFToMsg(orien, control.orientation);
224225
break;
225226
case 1:
226227
int_marker.name = sign>0 ? "max_y" : "min_y";
227228
int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
228229
int_marker.pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y();
229230
int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
230-
control.orientation.x = 0;
231-
control.orientation.y = 0;
232-
control.orientation.z = 1;
231+
orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
232+
orien.normalize();
233+
tf::quaternionTFToMsg(orien, control.orientation);
233234
break;
234235
default:
235236
int_marker.name = sign>0 ? "max_z" : "min_z";
236237
int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
237238
int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
238239
int_marker.pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z();
239-
control.orientation.x = 0;
240-
control.orientation.y = -1;
241-
control.orientation.z = 0;
240+
orien = tf::Quaternion(0.0, -1.0, 0.0, 1.0);
241+
orien.normalize();
242+
tf::quaternionTFToMsg(orien, control.orientation);
242243
break;
243244
}
244245

0 commit comments

Comments
 (0)