Add polygon clipping ops to utilities

p3iv_utils/package.xml

@@ -18,9 +18,11 @@
   <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-numpy</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-scipy</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-simplejson</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-shapely</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-numpy</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-scipy</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-simplejson</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-shapely</exec_depend>
 
 
   <export>

p3iv_utils/src/p3iv_utils/polygon_operations.py

@@ -0,0 +1,18 @@
+# This file is part of the P3IV Simulator (https://github.com/fzi-forschungszentrum-informatik/P3IV),
+# copyright by FZI Forschungszentrum Informatik, licensed under the BSD-3 license (see LICENSE file in main directory)
+
+import numpy as np
+from shapely.geometry import Point
+from shapely.geometry.polygon import Polygon
+
+
+class PolygonCalculation(object):
+    def __init__(self, right_bound, left_bound):
+        """Create a shapely-polygon"""
+        p = np.vstack([right_bound, left_bound[::-1]])
+        self.polygon = Polygon(p)
+
+    def __call__(self, point_xy):
+        """Check if point is inside polygon. Return boolean"""
+        point = Point(point_xy[0], point_xy[1])
+        return self.polygon.contains(point)