Conversion between Matrix coordinates and Stage coordinates

edit

Hi,

I'm trying to implement the A* algorithm for path planning. In order to do it, I'm using OccupancyGrid topic and Odom. So, first of all, I initialize my map as a matrix (using Python):

def createMap(robot):
    mapMatrix =  numpy.asarray(robot.MAP_MSG.data).reshape((robot.MAP_MSG.info.height,robot.MAP_MSG.info.width))
    RESOLUTION = robot.MAP_MSG.info.resolution
    ORIGIN_Y = -(robot.MAP_MSG.info.height * robot.MAP_MSG.info.resolution / 2)
    ORIGIN_X = -(robot.MAP_MSG.info.width * robot.MAP_MSG.info.resolution / 2)
    MAP_HEIGHT = robot.MAP_MSG.info.height
    MAP_WIDTH = robot.MAP_MSG.info.width
    return [MAP_HEIGHT, MAP_WIDTH, ORIGIN_Y, ORIGIN_X, RESOLUTION, mapMatrix]

To convert from map (odom) coordinates to matrix coordinates, I created this method:

def mapToMatrix(point, ORIGIN_X, ORIGIN_Y, RESOLUTION): 
    return (int((point[0] - ORIGIN_X)/RESOLUTION), int((point[1] - ORIGIN_Y)/RESOLUTION))

So, given a start point (i.e., -7, -7) and a goal point (i.e. 7, 7), I can convert both into matrix coordinates and run my A* algorithm to find a feasible path on my mapMatrix.

My A* algorithm returns a list of tuples, in which each element is a position in the matrix that the robot will visit. So all I need to do now is to transform those coordinates into "map" coordinates. To do it, I created this method:

 def matrixToMap(point, ORIGIN_X, ORIGIN_Y, RESOLUTION):
    return (point[0]*RESOLUTION + ORIGIN_X, point[1]*RESOLUTION + ORIGIN_Y)

The problem is that my pioneer robot is passing through the obstacles, even if the path is feasible. For example, on the map on the image below,

I got a feasible path (the 4 other images, where @ are obstacles and * are part of the path)

(if it suits anyone, the map file is attached here: https://drive.google.com/open?id=1HoI... )

So I'm wondering if there's anything wrong with my transformation from Matrix coordinates to Map Coordinates. The transformation from Map Coordinates to Matrix Coordinates seems to be working quite well, since my robot starts at (-7,-7) (which is something link 31,31 in matrix) and ends in (7,7) (which is something like 482, 482 in matrix). I'm using stage for simulations, python for my scripts and, ROS Melodic Morenia.