From ed7269d27ecbcea11696e2c0a9c6454726737d7d Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Mon, 27 Nov 2023 13:47:19 +0100
Subject: [PATCH] fix(moveit_server): fix add_plane service

This commit ensures that the moveit server `add_plane` service does not
crash when no normal is given.
---
 .../src/panda_gazebo/common/helpers.py        | 46 +++++++++++++++++++
 .../src/panda_gazebo/core/moveit_server.py    | 24 ++++------
 2 files changed, 56 insertions(+), 14 deletions(-)

diff --git a/panda_gazebo/src/panda_gazebo/common/helpers.py b/panda_gazebo/src/panda_gazebo/common/helpers.py
index 4c9f6afb..02c9e8d9 100644
--- a/panda_gazebo/src/panda_gazebo/common/helpers.py
+++ b/panda_gazebo/src/panda_gazebo/common/helpers.py
@@ -429,6 +429,52 @@ def normalize_quaternion(quaternion):
     return quaternion
 
 
+def vector_norm(vector):
+    """Calculates the norm of a vector.
+
+    Args:
+        vector (union[list, tuple, np.array]): A vector.
+
+    Returns:
+        float: The norm of the vector.
+
+    Raises:
+        TypeError: If vector is not of type list or tuple.
+    """
+    if not isinstance(vector, (list, tuple, np.ndarray)):
+        raise TypeError("vector must be of type list, tuple or numpy array")
+
+    vector = np.array(vector)
+    return np.linalg.norm(vector)
+
+
+def normalize_vector(vector, force=True):
+    """Normalizes a given vector.
+
+    Args:
+        vector (union[list, tuple]): A vector.
+        force (bool): Whether to force the vector to have a unit length if the norm is
+            zero. Defaults to ``True``.
+
+    Returns:
+        list: The normalized vector.
+
+    Raises:
+        TypeError: If vector is not of type list or tuple.
+    """
+    if not isinstance(vector, (list, tuple, np.ndarray)):
+        raise TypeError("vector must be of type list or tuple or numpy array")
+
+    vector = np.array(vector)
+    norm = vector_norm(vector)
+    if norm == 0:
+        if force:
+            return [0.0, 0.0, 1.0]
+        rospy.logwarn("Vector could not be normalized since the norm is zero.")
+        return list(vector)
+    return list(vector / norm)
+
+
 def ros_exit_gracefully(shutdown_msg=None, exit_code=0):
     """Shuts down the ROS node wait until it is shutdown and exits the script.
 
diff --git a/panda_gazebo/src/panda_gazebo/core/moveit_server.py b/panda_gazebo/src/panda_gazebo/core/moveit_server.py
index 753977a5..6e84ae4f 100644
--- a/panda_gazebo/src/panda_gazebo/core/moveit_server.py
+++ b/panda_gazebo/src/panda_gazebo/core/moveit_server.py
@@ -54,6 +54,7 @@
     quaternion_norm,
     ros_exit_gracefully,
     translate_moveit_error_code,
+    normalize_vector,
 )
 from panda_gazebo.exceptions import InputMessageInvalidError
 from panda_gazebo.srv import (
@@ -1884,30 +1885,25 @@ def _scene_add_plane_callback(self, add_plane_req):
         Returns:
             bool: Whether the plane was successfully added.
         """
-        pose_header = Header(
-            frame_id=add_plane_req.frame_id if add_plane_req.frame_id else "world"
-        )
+        pose_header = Header(frame_id=add_plane_req.frame_id or "world")
         plane_pose = Pose(
             position=add_plane_req.pose.position,
-            orientation=normalize_quaternion(add_plane_req.pose.orientation),
+            orientation=(add_plane_req.pose.orientation),
         )
+        pose_stamped = PoseStamped(header=pose_header, pose=plane_pose)
 
         # Send request.
         resp = AddPlaneResponse()
         try:
             self.scene.add_plane(
-                add_plane_req.name if add_plane_req.name else "plane",
-                PoseStamped(header=pose_header, pose=plane_pose)
-                if add_plane_req.pose
-                else PoseStamped(
-                    header=pose_header, pose=Pose(orientation=Quaternion(0, 0, 0, 1))
-                ),
-                normal=add_plane_req.normal if add_plane_req.normal else (0, 0, 1),
-                offset=add_plane_req.offset if add_plane_req.offset else 0,
+                add_plane_req.name or "plane",
+                pose_stamped,
+                normal=normalize_vector(add_plane_req.normal, force=True),
+                offset=add_plane_req.offset,
             )
-        except Exception:
+        except Exception as e:
             resp.success = False
-            resp.message = "Plane could not be added"
+            resp.message = "Plane could not be added because: %s" % e
             return resp
 
         # Return response.