def __init__(self,
filename=None,
replay=False,
assertWrite=True,
ipPair=None):
if ipPair is not None:
setIPs(*ipPair)
self.node = NodeROS(subscribe=[
'/imu/data', '/imu/mag', '/imu/rpy', '/imu/temperature',
'/husky/data/encoders', '/husky/data/power_status',
'/husky/data/safety_status', '/joy', '/husky/data/system_status'
],
publish=['/husky/cmd_vel'],
heartbeat='/husky/data/encoders',
filename=filename,
replay=replay,
assertWrite=assertWrite)
self.speed = 0.0
self.angularSpeed = 0.0
self.enc = None # unknown
self.time = None
self.power = None
self.imu = None
self.mag = None
self.azimuth = None
self.emergencyStopPressed = None
self.greenPressed = None
self.redPressed = None
self.joyAxis = (0, 0, 0, 0, 0, 0)
self.update()
def __init__( self, filename=None, replay=False, assertWrite=True, ipPair=None ):
if ipPair is not None:
setIPs( *ipPair )
self.node = NodeROS(
subscribe=['/imu/data', '/imu/mag', '/imu/rpy', '/imu/temperature',
'/husky/data/encoders',
'/husky/data/power_status',
'/husky/data/safety_status',
'/joy',
'/husky/data/system_status'],
publish=['/husky/cmd_vel'],
heartbeat='/husky/data/encoders',
filename=filename, replay=replay, assertWrite=assertWrite )
self.speed = 0.0
self.angularSpeed = 0.0
self.enc = None # unknown
self.time = None
self.power = None
self.imu = None
self.mag = None
self.azimuth = None
self.emergencyStopPressed = None
self.greenPressed = None
self.redPressed = None
self.joyAxis = (0,0, 0,0, 0,0)
self.update()
def __init__(self, metalog=None, assertWrite=True):
self.node = NodeROS(subscribe=[
'/imu/data', '/imu/mag', '/imu/rpy', '/imu/temperature',
'/husky/data/encoders', '/husky/data/power_status',
'/husky/data/safety_status', '/joy', '/husky/data/system_status'
],
publish=['/husky/cmd_vel'],
heartbeat='/husky/data/encoders',
metalog=metalog,
assertWrite=assertWrite)
self.speed = 0.0
self.angularSpeed = 0.0
self.enc = None # unknown
self.time = None
self.imu = None
self.mag = None
self.azimuth = None
self.greenPressed = None
self.redPressed = None
self.joyAxis = (0, 0, 0, 0, 0, 0)
self.update()
def __init__( self, metalog=None, assertWrite=True ):
self.node = NodeROS(
subscribe=['/imu/data', '/imu/mag', '/imu/rpy', '/imu/temperature',
'/husky/data/encoders',
'/husky/data/power_status',
'/husky/data/safety_status',
'/joy',
'/husky/data/system_status'],
publish=['/husky/cmd_vel'],
heartbeat='/husky/data/encoders',
metalog=metalog, assertWrite=assertWrite )
self.speed = 0.0
self.angularSpeed = 0.0
self.enc = None # unknown
self.time = None
self.imu = None
self.mag = None
self.azimuth = None
self.greenPressed = None
self.redPressed = None
self.joyAxis = (0,0, 0,0, 0,0)
self.update()
class HuskyROS:
def __init__(self,
filename=None,
replay=False,
assertWrite=True,
ipPair=None):
if ipPair is not None:
setIPs(*ipPair)
self.node = NodeROS(subscribe=[
'/imu/data', '/imu/mag', '/imu/rpy', '/imu/temperature',
'/husky/data/encoders', '/husky/data/power_status',
'/husky/data/safety_status', '/joy', '/husky/data/system_status'
],
publish=['/husky/cmd_vel'],
heartbeat='/husky/data/encoders',
filename=filename,
replay=replay,
assertWrite=assertWrite)
self.speed = 0.0
self.angularSpeed = 0.0
self.enc = None # unknown
self.time = None
self.power = None
self.imu = None
self.mag = None
self.azimuth = None
self.emergencyStopPressed = None
self.greenPressed = None
self.redPressed = None
self.joyAxis = (0, 0, 0, 0, 0, 0)
self.update()
def update(self):
self.node.cmdList.append(
('/husky/cmd_vel', (self.speed, self.angularSpeed)))
status = self.node.update()
for topic, data in status:
if topic == '/joy':
self.joyAxis, buttons = data
self.greenPressed = (buttons[1] == 1)
self.redPressed = (buttons[2] == 1)
if topic == '/imu/data':
q0, q1, q2, q3 = data # quaternion
# print "[%.2f %.2f %.2f]" % (1-2*(q2*q2+q3*q3), 2*(q1*q2-q0*q3), 2*(q0*q2+q1*q3)),
# print "[%.2f %.2f %.2f]" % (2*(q1*q2+q0*q3), 1-2*(q1*q1+q3*q3), 2*(q2*q3-q0*q1)),
# print "[%.2f %.2f %.2f]" % (2*(q1*q3-q0*q2), 2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2))
# a2 = math.acos(q0) # q0 = cos(alpha/2)
# sinA2 = math.sin(a2)
# bx = math.acos(q1/sinA2) # q1 = sin(alpha/2)*cos(bx)
# by = math.acos(q2/sinA2)
# bz = math.acos(q3/sinA2)
# self.imu = data[:]
# print bx, by, bz #math.degrees(2*a2)
self.imu = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 *
(q1 * q1 + q2 * q2)) # along X axis
# self.imu = math.degrees( math.asin(2*(q0*q2-q3*q1)) ) # along Y axis ... max 5deg
# self.imu = math.degrees( math.atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3)) ) # along Z axis ~80 deg
if topic == '/imu/mag':
self.mag = data[:]
self.azimuth = math.atan2(data[2], data[0])
if topic == '/husky/data/encoders':
self.time = data[0]
self.enc = data[1][0], data[2][0] # only distance traveled
if topic == '/husky/data/power_status':
self.power = data
if topic == '/husky/data/safety_status':
self.emergencyStopPressed = data[
1] # ignored 16 unknown bits in data[0]
def setSpeedPxPa(self, speed, angularSpeed):
self.speed = speed
self.angularSpeed = angularSpeed
def goStraight(self, dist, maxSpeed=0.1):
"go forward for given distance"
print "GO!", dist, maxSpeed
assert self.enc != None
prevEncL, prevEncR = self.enc
startTime = self.time
while ((self.enc[0] - prevEncL) +
(self.enc[1] - prevEncR)) / 2.0 < dist:
self.setSpeedPxPa(maxSpeed, 0)
self.update()
self.setSpeedPxPa(0, 0)
self.update()
print "TIME", self.time - startTime
def turn(self, angle):
"go forward for given distance"
WIDTH = 0.55
assert self.enc != None
prevEncL, prevEncR = self.enc
startTime = self.time
prevImu = self.imu
# while abs((self.enc[0]-prevEncL) - (self.enc[1]-prevEncR)) < WIDTH*angle: # unusable
while abs(normalizeAnglePIPI(self.imu - prevImu)) < angle:
self.setSpeedPxPa(0.0, 0.3)
self.update()
self.setSpeedPxPa(0, 0)
self.update()
print "TIME", self.time - startTime, math.degrees(
normalizeAnglePIPI(self.imu - prevImu))
def turnToAzimuth(self, azimuth):
print "AZI", math.degrees(self.azimuth), math.degrees(azimuth)
while abs(
normalizeAnglePIPI(self.azimuth - azimuth)) > math.radians(2):
self.setSpeedPxPa(0.0, 0.3)
self.update()
self.setSpeedPxPa(0, 0)
self.update()
def wait(self, seconds):
startTime = self.time
while startTime + seconds > self.time:
self.setSpeedPxPa(0.0, 0.0)
self.update()
class HuskyROS:
def __init__( self, filename=None, replay=False, assertWrite=True, ipPair=None ):
if ipPair is not None:
setIPs( *ipPair )
self.node = NodeROS(
subscribe=['/imu/data', '/imu/mag', '/imu/rpy', '/imu/temperature',
'/husky/data/encoders',
'/husky/data/power_status',
'/husky/data/safety_status',
'/joy',
'/husky/data/system_status'],
publish=['/husky/cmd_vel'],
heartbeat='/husky/data/encoders',
filename=filename, replay=replay, assertWrite=assertWrite )
self.speed = 0.0
self.angularSpeed = 0.0
self.enc = None # unknown
self.time = None
self.power = None
self.imu = None
self.mag = None
self.azimuth = None
self.emergencyStopPressed = None
self.greenPressed = None
self.redPressed = None
self.joyAxis = (0,0, 0,0, 0,0)
self.update()
def update( self ):
self.node.cmdList.append( ('/husky/cmd_vel', (self.speed,self.angularSpeed)) )
status = self.node.update()
for topic,data in status:
if topic == '/joy':
self.joyAxis, buttons = data
self.greenPressed = (buttons[1]==1)
self.redPressed = (buttons[2]==1)
if topic == '/imu/data':
q0,q1,q2,q3 = data # quaternion
# print "[%.2f %.2f %.2f]" % (1-2*(q2*q2+q3*q3), 2*(q1*q2-q0*q3), 2*(q0*q2+q1*q3)),
# print "[%.2f %.2f %.2f]" % (2*(q1*q2+q0*q3), 1-2*(q1*q1+q3*q3), 2*(q2*q3-q0*q1)),
# print "[%.2f %.2f %.2f]" % (2*(q1*q3-q0*q2), 2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2))
# a2 = math.acos(q0) # q0 = cos(alpha/2)
# sinA2 = math.sin(a2)
# bx = math.acos(q1/sinA2) # q1 = sin(alpha/2)*cos(bx)
# by = math.acos(q2/sinA2)
# bz = math.acos(q3/sinA2)
# self.imu = data[:]
# print bx, by, bz #math.degrees(2*a2)
self.imu = math.atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2)) # along X axis
# self.imu = math.degrees( math.asin(2*(q0*q2-q3*q1)) ) # along Y axis ... max 5deg
# self.imu = math.degrees( math.atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3)) ) # along Z axis ~80 deg
if topic == '/imu/mag':
self.mag = data[:]
self.azimuth = math.atan2( data[2], data[0] )
if topic == '/husky/data/encoders':
self.time = data[0]
self.enc = data[1][0], data[2][0] # only distance traveled
if topic == '/husky/data/power_status':
self.power = data
if topic == '/husky/data/safety_status':
self.emergencyStopPressed = data[1] # ignored 16 unknown bits in data[0]
def setSpeedPxPa( self, speed, angularSpeed ):
self.speed = speed
self.angularSpeed = angularSpeed
def goStraight( self, dist, maxSpeed=0.1 ):
"go forward for given distance"
print "GO!", dist, maxSpeed
assert self.enc != None
prevEncL, prevEncR = self.enc
startTime = self.time
while ((self.enc[0]-prevEncL) + (self.enc[1]-prevEncR))/2.0 < dist:
self.setSpeedPxPa( maxSpeed, 0 )
self.update()
self.setSpeedPxPa( 0, 0 )
self.update()
print "TIME", self.time-startTime
def turn( self, angle ):
"go forward for given distance"
WIDTH = 0.55
assert self.enc != None
prevEncL, prevEncR = self.enc
startTime = self.time
prevImu = self.imu
# while abs((self.enc[0]-prevEncL) - (self.enc[1]-prevEncR)) < WIDTH*angle: # unusable
while abs( normalizeAnglePIPI(self.imu - prevImu) ) < angle:
self.setSpeedPxPa( 0.0, 0.3 )
self.update()
self.setSpeedPxPa( 0, 0 )
self.update()
print "TIME", self.time-startTime, math.degrees( normalizeAnglePIPI(self.imu - prevImu) )
def turnToAzimuth( self, azimuth ):
print "AZI", math.degrees( self.azimuth ), math.degrees( azimuth )
while abs( normalizeAnglePIPI(self.azimuth - azimuth) ) > math.radians(2):
self.setSpeedPxPa( 0.0, 0.3 )
self.update()
self.setSpeedPxPa( 0, 0 )
self.update()
def wait( self, seconds ):
startTime = self.time
while startTime + seconds > self.time:
self.setSpeedPxPa( 0.0, 0.0 )
self.update()
