ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to convert laserscan measurements of ROS into values of Occupancy grid?

asked 2023-04-24 05:16:36 -0500

terminxd gravatar image

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
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2023-04-25 09:56:26 -0500

alexnavtt gravatar image

It looks like you forgot to account for the theta value that you passed in. This would have the effect of always behaving as if the robot had never rotated. I think all you need to do is add theta to the math.sin and math.cos terms to get the correct result.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2023-04-24 05:16:36 -0500

Seen: 64 times

Last updated: Apr 25 '23