Check if given orientation is valid before moving GUI camera

[sdhar04]

When I give an invalid quaternion, say (0,0,0,0), to the GUI camera through gz service -s /gui/move_to/pose --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean --timeout 500 --req 'pose: {position:{x: 0, y:0,z:0}, orientation:{x:0, y:0, z:0, w:0}}', Gazebo moves the camera to the given position without changing the orientation, and then crashes with the following messages:

gz sim gui: /home/jenkins/workspace/ogre-2.3-debbuilder/repo/OgreMain/include/Math/Array/SSE2/Single/OgreMathlibSSE2.h:136: static Ogre::ArrayReal Ogre::MathlibSSE2::Cmov4(Ogre::ArrayReal, Ogre::ArrayReal, Ogre::ArrayMaskR): Assertion `_mm_movemask_ps( _mm_cmpeq_ps( arg1, arg1 ) ) == 0x0f && _mm_movemask_ps( _mm_cmpeq_ps( arg2, arg2 ) ) == 0x0f && "Passing NaN values to CMov4"' failed.
Stack trace (most recent call last) in thread 2774:
gazebosim/gz-math#10   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
gazebosim/gz-math#9    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fe802b62352, in clone
gazebosim/gz-math#8    Object "/lib/x86_64-linux-gnu/libpthread.so.0", at 0x7fe802a28608, in 
gazebosim/gz-math#7    Object "/usr/lib/x86_64-linux-gnu/OGRE-2.3/libOgreNextMain.so.2.3.1", at 0x7fe7a2c2bbe9, in 
gazebosim/gz-math#6    Object "/usr/lib/x86_64-linux-gnu/OGRE-2.3/libOgreNextMain.so.2.3.1", at 0x7fe7a2c2bb39, in Ogre::SceneManager::_updateWorkerThread(Ogre::ThreadHandle*)
gazebosim/gz-math#5    Object "/usr/lib/x86_64-linux-gnu/OGRE-2.3/libOgreNextMain.so.2.3.1", at 0x7fe7a2c27518, in Ogre::SceneManager::updateAllTransformsThread(Ogre::UpdateTransformRequest const&, unsigned long)
gazebosim/gz-math#4    Object "/usr/lib/x86_64-linux-gnu/OGRE-2.3/libOgreNextMain.so.2.3.1", at 0x7fe7a2b87713, in Ogre::Node::updateAllTransforms(unsigned long, Ogre::Transform)
gazebosim/gz-math#3    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fe802a76fd5, in __assert_fail
gazebosim/gz-math#2    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fe802a65728, in 
gazebosim/gz-math#1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fe802a65858, in abort
#0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fe802a8600b, in gsignal
Aborted (Signal sent by tkill() 2630 1000)

These crashes also happen when a given argument is a very large number, but it also gives additional error messages saying [libprotobuf ERROR google/protobuf/text_format.cc:307] Error parsing text-format gz.msgs.GUICamera: 1:49: Integer out of range

Is it an option to add checks to see if the given quaternion is valid? If yes, then we could either publish error messages in the terminal reporting invalid arguments, or give a default orientation to the camera along with publishing similar error messages.

https://github.com/gazebosim/gz-math/assets/126956357/5f1b6aee-f402-49f7-9ed3-1a14819e83e0

https://github.com/gazebosim/gz-math/assets/126956357/f27a6792-c6c2-4b68-b960-7b2e5275ccb8

[azeey]

I'll move this issue to gz-gui since I think the validity check should be done there.

[pritesh299]

@sdhar04 can you mention which version of Ros you were using? I need it to recreate the issue.

[sdhar04]

@pritesh299 I was using ROS Noetic on Ubuntu 20.04.

[thorstink]

Just bumped into this as well using gz-garden, no ros, just running a gz::server, but simply setting the pose with a wrong quaternion, the following gives me a crash:

  const auto some pose = gz::math::Pose3d(0,0,0,0,0,0,0);
  auto model = Model(some_entity);
  model.SetWorldPoseCmd(_ecm, pose);

while the following does not:

  const auto some pose = gz::math::Pose3d(0,0,0,1,0,0,0);
  auto model = Model(some_entity);
  model.SetWorldPoseCmd(_ecm, pose);

explicit crash-error:

my_executable: /home/jenkins/workspace/ogre-2.3-debbuilder/repo/OgreMain/include/Math/Array/SSE2/Single/OgreMathlibSSE2.h:136: static Ogre::ArrayReal Ogre::MathlibSSE2::Cmov4(Ogre::ArrayReal, Ogre::ArrayReal, Ogre::ArrayMaskR): Assertion `_mm_movemask_ps( _mm_cmpeq_ps( arg1, arg1 ) ) == 0x0f && _mm_movemask_ps( _mm_cmpeq_ps( arg2, arg2 ) ) == 0x0f && "Passing NaN values to CMov4"' failed.