def laserScan2LaserData(scan):
'''
Translates from ROS LaserScan to JderobotTypes LaserData.
@param scan: ROS LaserScan to translate
@type scan: LaserScan
@return a LaserData translated from scan
'''
laser = LaserData()
laser.values = scan.ranges
'''
ROS Angle Map JdeRobot Angle Map
0 PI/2
| |
| |
PI/2 --------- -PI/2 PI --------- 0
| |
| |
'''
laser.minAngle = scan.angle_min + PI / 2
laser.maxAngle = scan.angle_max + PI / 2
laser.maxRange = scan.range_max
laser.minRange = scan.range_min
laser.timeStamp = scan.header.stamp.secs + (scan.header.stamp.nsecs * 1e-9)
return laser
def laserScan2LaserData(scan):
'''
Translates from ROS LaserScan to JderobotTypes LaserData.
@param scan: ROS LaserScan to translate
@type scan: LaserScan
@return a LaserData translated from scan
'''
laser = LaserData()
laser.values = scan.ranges
'''
ROS Angle Map JdeRobot Angle Map
0 PI/2
| |
| |
PI/2 --------- -PI/2 PI --------- 0
| |
| |
'''
laser.minAngle = scan.angle_min + pi/2
laser.maxAngle = scan.angle_max + pi/2
laser.maxRange = scan.range_max
laser.minRange = scan.range_min
laser.timeStamp = scan.header.stamp.secs + (scan.header.stamp.nsecs *1e-9)
return laser
def update(self):
'''
Updates LaserData.
'''
if self.hasproxy():
laserD = LaserData()
values = []
data = self.proxy.getLaserData()
#laserD.values = laser.distanceData
for i in range(data.numLaser):
values.append(data.distanceData[i] / 1000.0)
laserD.maxAngle = data.maxAngle
laserD.minAngle = data.minAngle
laserD.maxRange = data.maxRange
laserD.minRange = data.minRange
laserD.values = values
self.lock.acquire()
self.laser = laserD
self.lock.release()
def update(self):
'''
Updates LaserData.
'''
if self.hasproxy():
laserD = LaserData()
values = []
data = self.proxy.getLaserData()
#laserD.values = laser.distanceData
for i in range (data.numLaser):
values.append(data.distanceData[i] / 1000.0)
laserD.maxAngle = data.maxAngle
laserD.minAngle = data.minAngle
laserD.maxRange = data.maxRange
laserD.minRange = data.minRange
laserD.values = values
self.lock.acquire()
self.laser = laserD
self.lock.release()