Conversion between Matrix coordinates and Stage coordinates

asked 2018-09-25 09:10:35 -0500

woz gravatar image


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(,
    ORIGIN_Y = -( * / 2)
    ORIGIN_X = -( * / 2)

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,

image description

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

image description

image description

image description

image description

(if it suits anyone, the map file is attached here: )

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.

edit retag flag offensive close merge delete



Not an answer, but when initially reading the title I thought "Matrix coordinates? Neo?".

gvdhoorn gravatar image gvdhoorn  ( 2018-09-25 10:38:05 -0500 )edit

Well, at least there's a bit of humour after all :P

woz gravatar image woz  ( 2018-09-25 15:53:40 -0500 )edit