mapping.py

#!/usr/bin/env python3

"""
    # {Alessandro Bassi}
    # {student id}
    # {alebas@kth.se}
"""

# Python standard library
from math import cos, sin, atan2, fabs, sqrt

# Numpy
import numpy as np

# "Local version" of ROS messages
from local.geometry_msgs import PoseStamped, Quaternion
from local.sensor_msgs import LaserScan
from local.nav_msgs import OccupancyGrid
from local.map_msgs import OccupancyGridUpdate

from grid_map import GridMap

old_grid = None


class Mapping:
    def __init__(self, unknown_space, free_space, c_space, occupied_space,
                 radius, optional=None):
        self.unknown_space = unknown_space
        self.free_space = free_space
        self.c_space = c_space
        self.occupied_space = occupied_space
        self.allowed_values_in_map = {"self.unknown_space": self.unknown_space,
                                      "self.free_space": self.free_space,
                                      "self.c_space": self.c_space,
                                      "self.occupied_space": self.occupied_space}
        self.radius = radius
        self.__optional = optional

    def get_yaw(self, q):
        """Returns the Euler yaw from a quaternion.
        :type q: Quaternion
        """
        return atan2(2 * (q.w * q.z + q.x * q.y),
                     1 - 2 * (q.y * q.y + q.z * q.z))

    def raytrace(self, start, end):
        """Returns all cells in the grid map that has been traversed
        from start to end, including start and excluding end.
        start = (x, y) grid map index
        end = (x, y) grid map index
        """
        (start_x, start_y) = start
        (end_x, end_y) = end
        x = start_x
        y = start_y
        (dx, dy) = (fabs(end_x - start_x), fabs(end_y - start_y))
        n = dx + dy
        x_inc = 1
        if end_x <= start_x:
            x_inc = -1
        y_inc = 1
        if end_y <= start_y:
            y_inc = -1
        error = dx - dy
        dx *= 2
        dy *= 2

        traversed = []
        for i in range(0, int(n)):
            traversed.append((int(x), int(y)))

            if error > 0:
                x += x_inc
                error -= dy
            else:
                if error == 0:
                    traversed.append((int(x + x_inc), int(y)))
                y += y_inc
                error += dx

        return traversed

    def add_to_map(self, grid_map, x, y, value):
        """Adds value to index (x, y) in grid_map if index is in bounds.
        Returns weather (x, y) is inside grid_map or not.
        """
        if value not in self.allowed_values_in_map.values():
            raise Exception("{0} is not an allowed value to be added to the map. "
                            .format(value) + "Allowed values are: {0}. "
                            .format(self.allowed_values_in_map.keys()) +
                            "Which can be found in the '__init__' function.")

        if self.is_in_bounds(grid_map, x, y):
            grid_map[x, y] = value
            return True
        return False

    def is_in_bounds(self, grid_map, x, y):
        """Returns weather (x, y) is inside grid_map or not."""
        if x >= 0 and x < grid_map.get_width():
            if y >= 0 and y < grid_map.get_height():
                return True
        return False

    def update_map(self, grid_map, pose, scan):

        # Current yaw of the robot
        robot_yaw = self.get_yaw(pose.pose.orientation)
        # The origin of the map [m, m, rad]. This is the real-world pose of the
        # cell (0,0) in the map.
        origin = grid_map.get_origin()
        # The map resolution [m/cell]
        resolution = grid_map.get_resolution()
        global old_grid

        # Whe want to add the measures to the map  

        lbx = float('inf')
        ubx = - float('inf')
        lby = float('inf')
        uby = -float('inf')

        for i in range(len(scan.ranges)):
            # Check if the measure is valid 
            if scan.ranges[i] is None:
                print(f"ERROR IN UPDATE_MAP: the mesure is None")
                continue 
            #Extracting the angles and measures
            theta = scan.angle_min + scan.angle_increment * i
            measure = scan.ranges[i]
            #Check if the mesure is bounds 
            if measure <= scan.range_min or measure >= scan.range_max:
                continue 
            #Transformation from polar coordinates to robot relative cartesia coordinates 
            x_rel = measure * cos(theta)
            y_rel = measure * sin(theta)

            #Transformation from relative coordinates to base-frame coordinates
            x_base = pose.pose.position.x + (x_rel * cos(robot_yaw) - y_rel * sin(robot_yaw))
            y_base = pose.pose.position.y + (x_rel * sin(robot_yaw) + y_rel * cos(robot_yaw))

            #Conversion in OccupancyGrid coordinates 
            col = int((x_base - origin.position.x) / resolution)
            row = int((y_base - origin.position.y) / resolution)

            startx = int((pose.pose.position.x - origin.position.x) / resolution)
            starty = int((pose.pose.position.y - origin.position.y) / resolution)
            end = (col, row)
            start = (startx, starty)

            free_points = self.raytrace(start, end)
            
            #Adding the free points to the map
            for point in free_points:
                self.add_to_map(grid_map, point[0], point[1], self.free_space)
                if point[0] < lbx:
                    lbx = point[0]
                if point[0] > ubx:
                    ubx = point[0]
                if point[1] < lby:
                    lby = point[1]
                if point[1] > uby:
                    uby = point[1]

        for i in range(len(scan.ranges)):

            if scan.ranges[i] is None:
                print(f"ERROR IN UPDATE_MAP: the mesure is None")
                continue 
            #Extracting the angles and measures
            theta = scan.angle_min + scan.angle_increment * i
            measure = scan.ranges[i]
            #Check if the mesure is bounds 
            if measure <= scan.range_min or measure >= scan.range_max:
                continue 
            #Transformation from polar coordinates to robot relative cartesia coordinates 
            x_rel = measure * cos(theta)
            y_rel = measure * sin(theta)

            #Transformation from relative coordinates to base-frame coordinates
            x_base = pose.pose.position.x + (x_rel * cos(robot_yaw) - y_rel * sin(robot_yaw))
            y_base = pose.pose.position.y + (x_rel * sin(robot_yaw) + y_rel * cos(robot_yaw))

            #Conversion in OccupancyGrid coordinates 
            col = int((x_base - origin.position.x) / resolution)
            row = int((y_base - origin.position.y) / resolution)

            end = (col, row)

            #Adding the point occupied points to the map if it is in bound
            if self.is_in_bounds(grid_map, col, row):
                self.add_to_map(grid_map, col, row, self.occupied_space)
                if col < lbx:
                    lbx = col
                if col > ubx:
                    ubx = col
                if row < lby:
                    lby = row
                if row > uby:
                    uby = row

        # Only get the part that has been updated
        update = OccupancyGridUpdate()
        # The minimum x index in 'grid_map' that has been updated
        update.x = lbx
        # The minimum y index in 'grid_map' that has been updated
        update.y = lby
        # Maximum x index - minimum x index + 1
        update.width = ubx - lbx +1
        # Maximum y index - minimum y index + 1
        update.height = uby - lby +1
        # The map data inside the rectangle, in row-major order.
        update.data = []

        for row in range(update.x, update.x + update.width):
            for col in range(update.y, update.y + update.height):
                update.data.append(grid_map[row, col])
        return grid_map, update
    
    
    
    def inflate_map(self, grid_map):    
        global old_grid

        bias = [(x, y) for x in range(-self.radius, self.radius+1) for y in range(-self.radius, self.radius+1)]
        circle = [b for  b in bias if sqrt(b[0] **2 + b[1] ** 2) <= self.radius]
        
        for i in range  (grid_map.get_width()):
            for j in range(grid_map.get_height()):
                if grid_map[i, j] == self.occupied_space:
                    for el in circle:
                        px = i + el[0]
                        py = j + el[1]
                        if self.is_in_bounds(grid_map, px, py):
                            if grid_map[px, py] != self.occupied_space:
                                self.add_to_map(grid_map, px, py, self.c_space)
        # Return the inflated map
        return grid_map