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 __init__(self, ic, prefix):
'''
Laser Contructor.
Exits When it receives a Exception diferent to Ice.ConnectionRefusedException
@param ic: Ice Communicator
@param prefix: prefix name of client in config file
@type ic: Ice Communicator
@type prefix: String
'''
self.lock = threading.Lock()
self.laser = LaserData()
try:
base = ic.propertyToProxy(prefix + ".Proxy")
self.proxy = jderobot.LaserPrx.checkedCast(base)
prop = ic.getProperties()
self.update()
if not self.proxy:
print('Interface ' + prefix + ' not configured')
except Ice.ConnectionRefusedException:
print(prefix + ': connection refused')
except:
traceback.print_exc()
exit(-1)
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 __init__(self, topic):
'''
ListenerLaser Constructor.
@param topic: ROS topic to subscribe
@type topic: String
'''
self.topic = topic
self.data = LaserData()
self.sub = None
self.lock = threading.Lock()
self.start()
def __init__(self, topic):
"""
ListenerLaser Constructor.
@param topic: ROS topic to subscribe
@type topic: String
"""
self.ros_node = rclpy.create_node('laser_listener')
self.topic = topic
self.data = LaserData()
self.sub = None
self.lock = threading.Lock()
self.start()
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()