How to convert laserscan measurements of ROS into values of Occupancy grid?
I'm trying to pass the measurements of laserscan message into an occupancy grid. Each time, i know the exact position of robot in map (x_pos_t, y_pos_t). I have already initialized an empty occupancy grid. So, when i run the code the first laserscan measurements are placed good in occupancy grid, when i rotate the robot, misaligned values are inserted. Can you help me to fix the problem?
def mapping(x_pos_t , y_pos_t, theta, scan_msg, map_msg):
# convert laser scans to occupancy grid
for i, dist in enumerate(scan_msg.ranges):
if dist < scan_msg.range_max:
x = int((x_pos_t + dist * math.cos(scan_msg.angle_min + i * scan_msg.angle_increment)) / map_msg.info.resolution)
y = int((y_pos_t + dist * math.sin(scan_msg.angle_min + i * scan_msg.angle_increment)) / map_msg.info.resolution)
if x >= 0 and xr < map_msg.info.width and y >= 0 and y < map_msg.info.height:
map_msg.data[y * map_msg.info.width + x] = 100
return map_msg