diff --git a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py new file mode 100644 index 00000000000..a0939a255c4 --- /dev/null +++ b/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)