Protect against non-finite inputs on Node and OrbitViewController (#655)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
gazebosim · Jun 25, 2022 · d773877 · d773877
1 parent 859272a
commit d773877
Show file tree

Hide file tree

Showing 3 changed files with 77 additions and 8 deletions.
diff --git a/include/ignition/rendering/base/BaseNode.hh b/include/ignition/rendering/base/BaseNode.hh
@@ -336,16 +336,16 @@ namespace ignition
     template <class T>
     void BaseNode<T>::SetLocalPose(const math::Pose3d &_pose)
     {
-      math::Pose3d pose = _pose;
-      pose.Pos() = pose.Pos() - pose.Rot() * this->origin;
-
-      if (!pose.IsFinite())
+      if (!_pose.IsFinite())
       {
-        ignerr << "Unable to set pose of a node: "
-               << "non-finite (nan, inf) values detected." << std::endl;
+        ignerr << "Unable to set non-finite pose [" << _pose
+               << "] to node [" << this->Name() << "]" << std::endl;
         return;
       }
 
+      math::Pose3d pose = _pose;
+      pose.Pos() = pose.Pos() - pose.Rot() * this->origin;
+
       if (!initialLocalPoseSet)
       {
         this->initialLocalPose = pose;
@@ -380,6 +380,13 @@ namespace ignition
     template <class T>
     void BaseNode<T>::SetLocalPosition(const math::Vector3d &_position)
     {
+      if (!_position.IsFinite())
+      {
+        ignerr << "Unable to set non-finite position [" << _position
+               << "] to node [" << this->Name() << "]" << std::endl;
+        return;
+      }
+
       math::Pose3d pose = this->LocalPose();
       pose.Pos() = _position;
       this->SetLocalPose(pose);
@@ -411,6 +418,13 @@ namespace ignition
     template <class T>
     void BaseNode<T>::SetLocalRotation(const math::Quaterniond &_rotation)
     {
+      if (!_rotation.IsFinite())
+      {
+        ignerr << "Unable to set non-finite rotation [" << _rotation
+               << "] to node [" << this->Name() << "]" << std::endl;
+        return;
+      }
+
       math::Pose3d pose = this->LocalPose();
       pose.Rot() = _rotation;
       this->SetLocalPose(pose);
@@ -525,6 +539,12 @@ namespace ignition
     template <class T>
     void BaseNode<T>::SetOrigin(const math::Vector3d &_origin)
     {
+      if (!_origin.IsFinite())
+      {
+        ignerr << "Unable to set non-finite origin [" << _origin
+               << "] to node [" << this->Name() << "]" << std::endl;
+        return;
+      }
       this->origin = _origin;
     }
 

diff --git a/ogre2/src/Ogre2RayQuery.cc b/ogre2/src/Ogre2RayQuery.cc
@@ -79,8 +79,27 @@ void Ogre2RayQuery::SetFromCamera(const CameraPtr &_camera,
   Ogre::Ray ray =
       camera->ogreCamera->getCameraToViewportRay(screenPos.X(), screenPos.Y());
 
-  this->origin = Ogre2Conversions::Convert(ray.getOrigin());
-  this->direction = Ogre2Conversions::Convert(ray.getDirection());
+  auto originMath = Ogre2Conversions::Convert(ray.getOrigin());
+  if (originMath.IsFinite())
+  {
+    this->origin = originMath;
+  }
+  else
+  {
+    ignwarn << "Attempted to set non-finite origin from camera ["
+            << camera->Name() << "]" << std::endl;
+  }
+
+  auto directionMath = Ogre2Conversions::Convert(ray.getDirection());
+  if (directionMath.IsFinite())
+  {
+    this->direction = directionMath;
+  }
+  else
+  {
+    ignwarn << "Attempted to set non-finite direction from camera ["
+            << camera->Name() << "]" << std::endl;
+  }
 
   this->dataPtr->camera = camera;
 

diff --git a/src/OrbitViewController.cc b/src/OrbitViewController.cc
@@ -15,6 +15,8 @@
  *
  */
 
+#include <cmath>
+
 #include <ignition/common/Console.hh>
 
 #include "ignition/rendering/OrbitViewController.hh"
@@ -92,6 +94,13 @@ const math::Vector3d &OrbitViewController::Target() const
 //////////////////////////////////////////////////
 void OrbitViewController::Zoom(const double _value)
 {
+  if (!std::isfinite(_value))
+  {
+    ignerr << "Failed to zoom by non-finite value [" << _value << "]"
+           << std::endl;
+    return;
+  }
+
   if (!this->dataPtr->camera)
   {
     ignerr << "Camera is NULL" << std::endl;
@@ -113,12 +122,26 @@ void OrbitViewController::Zoom(const double _value)
 //////////////////////////////////////////////////
 void OrbitViewController::Pan(const math::Vector2d &_value)
 {
+  if (!_value.IsFinite())
+  {
+    ignerr << "Failed to pan by non-finite value [" << _value << "]"
+           << std::endl;
+    return;
+  }
+
   if (!this->dataPtr->camera)
   {
     ignerr << "Camera is NULL" << std::endl;
     return;
   }
 
+  if (!this->dataPtr->camera->WorldPosition().IsFinite())
+  {
+    ignerr << "Camera world position isn't finite ["
+           << this->dataPtr->camera->WorldPosition() << "]" << std::endl;
+    return;
+  }
+
   double viewportWidth = this->dataPtr->camera->ImageWidth();
   double viewportHeight = this->dataPtr->camera->ImageHeight();
 
@@ -148,6 +171,13 @@ void OrbitViewController::Pan(const math::Vector2d &_value)
 //////////////////////////////////////////////////
 void OrbitViewController::Orbit(const math::Vector2d &_value)
 {
+  if (!_value.IsFinite())
+  {
+    ignerr << "Failed to orbit by non-finite value [" << _value << "]"
+           << std::endl;
+    return;
+  }
+
   if (!this->dataPtr->camera)
   {
     ignerr << "Camera is NULL" << std::endl;