Ask Your Question
0

How do I convert odom and pose information to grid location on occupancygrid map [closed]

asked 2022-01-08 16:25:30 -0500

distro gravatar image

updated 2022-05-01 12:58:38 -0500

lucasw gravatar image

I am also using the occupancygrid.msgs type found in the topic /map to get the costmap information found in member data. I am trying to figure out how to convert odometry and pose type data(x,y) to grid index[x,y] if that makes sense(this is after I convert msg.data to a 2D array that represents the 2D map). I have tried using the resolution found in the .yaml file of created maps but that doesnt seem to work very well, unless I am getting the calculations wrong? If there is any confusion to my question please inform me so I may try to explain more.

The calculations I use to convert pose to pixel are here below(please not below is not actual code, its just i code form to delinate it from the rest of the text as calculation)

origin:[-10,-10,0] which are the x,y and yaw respectively
resolution: 0.05 meter/pixel

to convert poses to  [i,j]( pixel location/grid index)
i= ("pose.x" - (-10))/0.05
j= ("pose.y"-(-10))/0.05

It doesnt work completely accurately for some reason. For context, I made an algorithm that generates points that I use as goals for the robot to move to. some times , some of the goals are outside of free space of the map(that is to say the some goals appear in physical co-ordinate points whose cost value in the occupancy grid is either -1(unknown) or 100(occupied), What I wish my algorithm to do with these points is to completely skip them and not assign them as goals. Here is some pseudocode to explain what I thought would be an absolute solution:

import rospy
from nav_msgs.msg import OccupancyGrid
def getcell(msg):
    global grid
    h = np.array(msg.data)
    #grid = h.reshape(384,384)
    grid = h

rospy.init_node("controller_3")
rospy.Subscriber("/map",OccupancyGrid,getcell)

.........
ex = ((goal.pose.position.x-(-10))/0.05)
ey = ((goal.pose.position.y-(-10))/0.05)

if grid[math.ceil(ex*384 + ey)] == -1 or  grid[math.ceil(ex*384 + ey)] == 100:
    Skip the points

Points are still being created in grid cells that should be either -1 or 100 in cost value.

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by distro
close date 2022-01-29 03:46:15.314194

Comments

Your definition of the value assigned to a grid cell is not a common one. Usually these are used for path planning, and the value is considered the cost incurred by the robot's center point passing through that cell. Hence the name costmap. Cell values >= some magic value are considered LETHAL (e.g. an obstacle that no part of the robot footprint can pass thru.)

Regarding your question, edit your description and show us your math and we'll comment on it.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-01-08 17:15:22 -0500 )edit

@Mike Scheutzow Sorry, I am using a costmap, I messed up the terms, I have edited my question accordingly, I still have the same problem as before though.

distro gravatar image distro  ( 2022-01-12 11:20:02 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-01-13 06:51:29 -0500

Mike Scheutzow gravatar image

updated 2022-01-13 06:52:17 -0500

If the lower left corner of map is at coordinate (-10.0, -10.0), and pose is in the map coordinate system, then your math looks OK. Don't forget to truncate i, j to integer.

edit flag offensive delete link more

Comments

@Mike Scheutzow It doesnt work 100% of the time is the issue.I have updated my question to show you what I have done

distro gravatar image distro  ( 2022-01-14 03:02:34 -0500 )edit

Your conversion to a single array index is wrong. It should be: i = (width * y) + x

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-01-14 06:14:09 -0500 )edit

@Mike Scheutzow why is that the case? Also, if you have familiarity with using teb_local_planner, please look at another Question I asked.

distro gravatar image distro  ( 2022-01-14 11:56:16 -0500 )edit

This link says the array is row-major. According to the first 3 hits in google, this means "organized by rows".

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-01-14 14:34:36 -0500 )edit

@Mike Scheutzow would you know how to get x and y from i? its two unknown variables so I don't know.

distro gravatar image distro  ( 2022-02-07 03:06:01 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2022-01-08 16:25:30 -0500

Seen: 56 times

Last updated: Jan 14