我正试着把激光扫描信息的测量值输入到一个占领网格中。每次,我都知道机器人在Map中的准确位置(x_pos_t,y_pos_t)。我已经初始化了一个空的占用网格。因此,当我运行代码时,第一个laserscan测量值被放置在占用网格中,当我旋转机器人时,插入未对齐的值。你能帮我解决这个问题吗?
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
1条答案
按热度按时间zour9fqk1#
我不确定我是否正确理解了您的问题,但似乎您没有考虑机器人的方向(theta)时,将激光扫描测量值放入占用网格。在将激光扫描插入Map之前,应根据机器人的方向旋转激光扫描。下面是应用了旋转的函数的更新版本:
这应当有助于在机器人旋转时将激光扫描测量与占用网格对准。