Hi,
Probably many of us faced the fact, that the LIDAR sensors are very different in the simulation and on the real robot.
In the simulation, ranges has 360 slots and the ranges[0] is in front of the robot, and agles are all positive (from 0 radian to 2*PI radian).
On the real robot, ranges has 720 slots and ranges[360] is in front of the robot, and angles are from (nearly) -PI to PI.
I had a hard time to creating algorithms which work well in both environment.
What I did is a wrapper for LaserScan. It is always “shows” itself as the LaserScan in simulator.
So you do not need to write different codes for simulator and real robot.
Here is:
angle_utils.py
from math import pi
from typing import List
from sensor_msgs.msg import LaserScan
two_pi = 2 * pi
def index_from_rad(rad: float, angle_min: float, angle_max: float, angle_increment: float):
assert rad >= -two_pi and rad <= two_pi, "Invalid radian"
angle = rad - angle_min
if angle < 0:
angle += two_pi
if angle > two_pi:
angle -= two_pi
index = round(angle / angle_increment)
ranges = round((angle_max - angle_min)/angle_increment) + 1
return min(max(0, index), ranges - 1)
def index_from_grad(grad: int, rad_angle_min: float, rad_angle_max: float, rad_angle_increment: float):
rad = grad * two_pi / 360.0
return index_from_rad(rad, rad_angle_min, rad_angle_max, rad_angle_increment)
class UnifiedLaserScan():
def __init__(self, msg: LaserScan):
self._msg = msg
self.header = msg.header
self.angle_min = 0
self.angle_max = two_pi
self.angle_increment = two_pi/360.0
self.time_increment = msg.time_increment
self.scan_time = msg.scan_time
self.range_min = msg.range_min
self.range_max = msg.range_max
self.ranges = msg.ranges if len(msg.ranges) == 360 else self._get_ranges()
# TODO: intensities are not transformed
self.intensities = msg.intensities
def _get_ranges(self) -> List[float]:
return [
self._msg.ranges[
index_from_grad(i,
self._msg.angle_min, self._msg.angle_max,
self._msg.angle_increment)
]
for i in range(360)
]
A test file to see how it works:
test_angle_utils.py
from path_planner_server.angle_utils import UnifiedLaserScan, index_from_grad, index_from_rad
from math import pi
import pytest
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Header
def index_from_rad_simu(rad: float) -> int:
return index_from_rad(rad, 0.0, 6.28000020980835, 0.01749303564429283)
def index_from_rad_real_robot(rad: float) -> int:
return index_from_rad(rad, -3.1241390705108643, 3.1415927410125732, 0.008714509196579456)
def index_from_grad_simu(grad: int) -> int:
return index_from_grad(grad, 0.0, 6.28000020980835, 0.01749303564429283)
def index_from_grad_real_robot(grad: int) -> int:
return index_from_grad(grad, -3.1241390705108643, 3.1415927410125732, 0.008714509196579456)
def test_index_from_rad_simu():
assert index_from_rad_simu(0) == 0
assert index_from_rad_simu(pi/2) == 90
assert index_from_rad_simu(pi) == 180
assert index_from_rad_simu(3 * pi/2) == 269
assert index_from_rad_simu(-pi/2) == 269
def test_index_from_rad_real_robot():
assert index_from_rad_real_robot(0) == 358
assert index_from_rad_real_robot(pi/2) == 539
assert index_from_rad_real_robot(pi) == 719
assert index_from_rad_real_robot(3 * pi/2) == 178
assert index_from_rad_real_robot(-pi/2) == 178
def test_index_from_rad_out_of_range():
assert index_from_rad_simu(-2 * pi) == 0
with pytest.raises(AssertionError):
index_from_rad_simu(2 * pi + 0.01)
with pytest.raises(AssertionError):
index_from_rad_simu(-2 * pi - 0.01)
def test_index_from_grad_simu():
assert index_from_grad_simu(0) == 0
assert index_from_grad_simu(90) == 90
assert index_from_grad_simu(180) == 180
assert index_from_grad_simu(270) == 269
assert index_from_grad_simu(-90) == 269
def test_index_from_grad_real_robot():
assert index_from_grad_real_robot(0) == 358
assert index_from_grad_real_robot(90) == 539
assert index_from_grad_real_robot(180) == 719
assert index_from_grad_real_robot(270) == 178
assert index_from_grad_real_robot(-90) == 178
def test_index_from_grad_out_of_range():
assert index_from_grad_simu(-360) == 0
with pytest.raises(AssertionError):
index_from_grad_simu(361)
with pytest.raises(AssertionError):
index_from_grad_simu(-361)
def test_unified_laserscan_init_simu():
scan = LaserScan(angle_min=0.0, angle_max=2*pi, angle_increment=2*pi/360.0)
scan.ranges = [float(i) for i in range(360)]
uscan = UnifiedLaserScan(scan)
assert uscan.ranges[100] == 100.0
def test_unified_laserscan_init_robot():
scan = LaserScan(angle_min=-3.1241390705108643, angle_max=3.1415927410125732, angle_increment=0.008714509196579456)
scan.ranges = [float(i) for i in range(720)]
uscan = UnifiedLaserScan(scan)
assert uscan.ranges[-180] == 719
assert uscan.ranges[-90] == 178
assert uscan.ranges[0] == 358
assert uscan.ranges[90] == 539
assert uscan.ranges[180] == 719
assert uscan.ranges[270] == 178
assert uscan.ranges[359] == 356
How to use this:
def laser_callback(self, _msg: LaserScan):
msg = UnifiedLaserScan(_msg)
ranges = msg.ranges
self.get_logger().debug('indexes: front: %d, left: %d, back: %d, rigth: %d' % (0, 90, 180, 270))
self.get_logger().debug('ranges: front: %.2f, left: %.2f, back: %.2f, rigth: %0.2f' %
(ranges[0], ranges[90], ranges[180], ranges[270]))
self.get_logger().debug('ranges size:%d' % (len(ranges)))
with self.q.mutex:
self.q.queue.clear()
self.q.put(msg)
Hope it will help to others.
I do not understand, why the simulator and real robot’s LIDAR are different. I think, this is unnecessary and I can not imagine an environment where it would be helpful to creat a simulated robot which is different from the real one. I mean, the simulated will be obviously different, but I may try to do it as similar as possible to the real one.
Péter