Create footprint_collision_checker.py

nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py

@@ -0,0 +1,220 @@
+#! /usr/bin/env python3
+# Copyright 2021 Samsung Research America
+# Copyright 2022 Afif Swaidan
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+This is a Python3 API for a Footprint Collision Checker.
+
+It provides the needed methods to manipulate the coordinates
+and calculate the cost of a Footprint
+"""
+
+from math import cos, sin
+
+from geometry_msgs.msg import Point32, Polygon
+from nav2_simple_commander.costmap_2d import PyCostmap2D
+from nav2_simple_commander.line_iterator import LineIterator
+
+NO_INFORMATION = 255
+LETHAL_OBSTACLE = 254
+INSCRIBED_INFLATED_OBSTACLE = 253
+MAX_NON_OBSTACLE = 252
+FREE_SPACE = 0
+
+
+class FootprintCollisionChecker:
+    """
+    FootprintCollisionChecker.
+
+    FootprintCollisionChecker Class for getting the cost
+    and checking the collisions of a Footprint
+    """
+
+    def __init__(self):
+        """Initialize the FootprintCollisionChecker Object."""
+        self.costmap_ = None
+        pass
+
+    def footprintCost(self, footprint: Polygon):
+        """
+        Iterate over all the points in a footprint and check for collision.
+
+        Args
+        ----
+            footprint (Polygon): The footprint to calculate the collision cost for
+
+        Returns
+        -------
+            LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
+            footprint_cost (float): The maximum cost found in the footprint points
+
+        """
+        footprint_cost = 0.0
+        x1 = 0.0
+        y1 = 0.0
+
+        x0, y0 = self.worldToMapValidated(footprint.points[0].x, footprint.points[0].y)
+
+        if x0 is None or y0 is None:
+            return LETHAL_OBSTACLE
+
+        xstart = x0
+        ystart = y0
+
+        for i in range(len(footprint.points) - 1):
+            x1, y1 = self.worldToMapValidated(
+                footprint.points[i + 1].x, footprint.points[i + 1].y
+            )
+
+            if x1 is None or y1 is None:
+                return LETHAL_OBSTACLE
+
+            footprint_cost = max(float(self.lineCost(x0, x1, y0, y1)), footprint_cost)
+            x0 = x1
+            y0 = y1
+
+            if footprint_cost == LETHAL_OBSTACLE:
+                return footprint_cost
+
+        return max(float(self.lineCost(xstart, x1, ystart, y1)), footprint_cost)
+
+    def lineCost(self, x0, x1, y0, y1, step_size=0.5):
+        """
+        Iterate over all the points along a line and check for collision.
+
+        Args
+        ----
+            x0 (float): Abscissa of the initial point in map coordinates
+            y0 (float): Ordinate of the initial point in map coordinates
+            x1 (float): Abscissa of the final point in map coordinates
+            y1 (float): Ordinate of the final point in map coordinates
+            step_size (float): Optional, Increments' resolution, defaults to 0.5
+
+        Returns
+        -------
+            LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
+            line_cost (float): The maximum cost found in the line points
+
+        """
+        line_cost = 0.0
+        point_cost = -1.0
+        line_iterator = LineIterator(x0, y0, x1, y1, step_size)
+
+        while line_iterator.isValid():
+            point_cost = self.pointCost(
+                int(line_iterator.getX()), int(line_iterator.getY())
+            )
+
+            if point_cost == LETHAL_OBSTACLE:
+                return point_cost
+
+            if line_cost < point_cost:
+                line_cost = point_cost
+
+            line_iterator.advance()
+
+        return line_cost
+
+    def worldToMapValidated(self, wx: float, wy: float):
+        """
+        Get the map coordinate XY using world coordinate XY.
+
+        Args
+        ----
+            wx (float): world coordinate X
+            wy (float): world coordinate Y
+
+        Returns
+        -------
+            None: if coordinates are invalid
+            tuple of int: mx, my (if coordinates are valid)
+            mx (int): map coordinate X
+            my (int): map coordinate Y
+
+        """
+        if self.costmap_ is None:
+            raise ValueError(
+                'Costmap not specified, use setCostmap to specify the costmap first'
+            )
+        return self.costmap_.worldToMap(wx, wy)
+
+    def pointCost(self, x: int, y: int):
+        """
+        Get the cost of a point in the costmap using map coordinates XY.
+
+        Args
+        ----
+            mx (int): map coordinate X
+            my (int): map coordinate Y
+
+        Returns
+        -------
+            np.uint8: cost of a point
+
+        """
+        if self.costmap_ is None:
+            raise ValueError(
+                'Costmap not specified, use setCostmap to specify the costmap first'
+            )
+        return self.costmap_.getCostXY(x, y)
+
+    def setCostmap(self, costmap: PyCostmap2D):
+        """
+        Specify which costmap to use.
+
+        Args
+        ----
+            costmap (PyCostmap2D): costmap to use in the object's methods
+
+        Returns
+        -------
+            None
+
+        """
+        self.costmap_ = costmap
+        return None
+
+    def footprintCostAtPose(self, x: float, y: float, theta: float, footprint: Polygon):
+        """
+        Get the cost of a footprint at a specific Pose in map coordinates.
+
+        Args
+        ----
+            x (float): map coordinate X
+            y (float): map coordinate Y
+            theta (float): absolute rotation angle of the footprint
+            footprint (Polygon): the footprint to calculate its cost at the given Pose
+
+        Returns
+        -------
+            LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
+            footprint_cost (float): The maximum cost found in the footprint points
+
+        """
+        cos_th = cos(theta)
+        sin_th = sin(theta)
+        oriented_footprint = Polygon()
+
+        for i in range(len(footprint.points)):
+            new_pt = Point32()
+            new_pt.x = x + (
+                footprint.points[i].x * cos_th - footprint.points[i].y * sin_th
+            )
+            new_pt.y = y + (
+                footprint.points[i].x * sin_th + footprint.points[i].y * cos_th
+            )
+            oriented_footprint.points.append(new_pt)
+
+        return self.footprintCost(oriented_footprint)