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.
Originally posted by distro on ROS Answers with karma: 167 on 2022-01-08
Post score: 0
Original comments
Comment by Mike Scheutzow on 2022-01-08:
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.
Comment by distro on 2022-01-12:
@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.