def test_pub_sub_msgs():
tcp = ('127.0.0.1', 9001)
pub = zmq.Pub(tcp)
sub = zmq.Sub(['test'], tcp)
msgs = [
Msgs.Vector(),
Msgs.Quaternion(),
Msgs.Array(),
Msgs.IMU(),
Msgs.Dictionary(),
Msgs.Odom(),
Msgs.Joystick(),
Msgs.Twist(),
Msgs.Wrench()
]
for tmsg in msgs:
while True:
print(tmsg)
pub.pub('test', tmsg)
topic, msg = sub.recv()
if msg:
assert msg == tmsg
assert topic == b'test'
break
def read(self, verbose=False):
js = self.js
dt = self.sleep
ps4 = Msg.Joystick()
twist = Msg.Twist()
try:
sdl2.SDL_JoystickUpdate()
# left axis
x = sdl2.SDL_JoystickGetAxis(js, 0) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 1) / 32768
ps4.axes.leftStick = [x, y]
# right axis
x = sdl2.SDL_JoystickGetAxis(js, 2) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 5) / 32768
ps4.axes.rightStick = [x, y]
# other axes
ps4.axes.L2 = sdl2.SDL_JoystickGetAxis(js, 3) / 32768
ps4.axes.R2 = sdl2.SDL_JoystickGetAxis(js, 4) / 32768
# accels
x = sdl2.SDL_JoystickGetAxis(js, 6) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 7) / 32768
z = sdl2.SDL_JoystickGetAxis(js, 8) / 32768
ps4.axes.accels = [x, y, z]
# gyros
x = sdl2.SDL_JoystickGetAxis(js, 9) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 10) / 32768
z = sdl2.SDL_JoystickGetAxis(js, 11) / 32768
ps4.axes.gyros = [x, y, z]
# get buttons
ps4.buttons.s = sdl2.SDL_JoystickGetButton(js, 0)
ps4.buttons.x = sdl2.SDL_JoystickGetButton(js, 1)
ps4.buttons.o = sdl2.SDL_JoystickGetButton(js, 2)
ps4.buttons.t = sdl2.SDL_JoystickGetButton(js, 3)
ps4.buttons.L1 = sdl2.SDL_JoystickGetButton(js, 4)
ps4.buttons.R1 = sdl2.SDL_JoystickGetButton(js, 5)
ps4.buttons.L2 = sdl2.SDL_JoystickGetButton(js, 6)
ps4.buttons.R2 = sdl2.SDL_JoystickGetButton(js, 7)
ps4.buttons.share = sdl2.SDL_JoystickGetButton(js, 8)
ps4.buttons.options = sdl2.SDL_JoystickGetButton(js, 9)
ps4.buttons.L3 = sdl2.SDL_JoystickGetButton(js, 10)
ps4.buttons.R3 = sdl2.SDL_JoystickGetButton(js, 11)
ps4.buttons.ps = sdl2.SDL_JoystickGetButton(js, 12)
ps4.buttons.pad = sdl2.SDL_JoystickGetButton(js, 13)
# get hat
# [up right down left] = [1 2 4 8]
ps4.buttons.hat = sdl2.SDL_JoystickGetHat(js, 0)
# print('b 12', sdl2.SDL_JoystickGetButton(js, 12))
# print('b 13', sdl2.SDL_JoystickGetButton(js, 13))
#self.pub.pub('js', ps4)
x, y = ps4.axes.rightStick
twist.linear.set(x, y, 0)
x, y = ps4.axes.leftStick
twist.angular.set(x, y, 0)
# verbose = True
if verbose:
print(twist)
if (self.old_twist.angular
== twist.angular) and (self.old_twist.linear
== twist.linear):
# print('js no msg')
pass
else:
self.pub.pub(self.topic, twist)
self.old_twist = twist
# print('js message sent')
time.sleep(dt)
except (IOError, EOFError):
print('[-] Connection gone .... bye')
def run(self, verbose, rate):
js = self.js
dt = 1.0/rate
ps4 = Msg.Joystick()
while True:
try:
sdl2.SDL_JoystickUpdate()
# left axis
x = sdl2.SDL_JoystickGetAxis(js, 0) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 1) / 32768
ps4.axes.leftStick = [x, y]
# right axis
x = sdl2.SDL_JoystickGetAxis(js, 2) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 5) / 32768
ps4.axes.rightStick = [x, y]
# other axes
ps4.axes.L2 = sdl2.SDL_JoystickGetAxis(js, 3) / 32768
ps4.axes.R2 = sdl2.SDL_JoystickGetAxis(js, 4) / 32768
# accels
x = sdl2.SDL_JoystickGetAxis(js, 6) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 7) / 32768
z = sdl2.SDL_JoystickGetAxis(js, 8) / 32768
ps4.axes.accels = [x, y, z]
# gyros
x = sdl2.SDL_JoystickGetAxis(js, 9) / 32768
y = sdl2.SDL_JoystickGetAxis(js, 10) / 32768
z = sdl2.SDL_JoystickGetAxis(js, 11) / 32768
ps4.axes.gyros = [x, y, z]
# get buttons
ps4.buttons.s = sdl2.SDL_JoystickGetButton(js, 0)
ps4.buttons.x = sdl2.SDL_JoystickGetButton(js, 1)
ps4.buttons.o = sdl2.SDL_JoystickGetButton(js, 2)
ps4.buttons.t = sdl2.SDL_JoystickGetButton(js, 3)
ps4.buttons.L1 = sdl2.SDL_JoystickGetButton(js, 4)
ps4.buttons.R1 = sdl2.SDL_JoystickGetButton(js, 5)
ps4.buttons.L2 = sdl2.SDL_JoystickGetButton(js, 6)
ps4.buttons.R2 = sdl2.SDL_JoystickGetButton(js, 7)
ps4.buttons.share = sdl2.SDL_JoystickGetButton(js, 8)
ps4.buttons.options = sdl2.SDL_JoystickGetButton(js, 9)
ps4.buttons.L3 = sdl2.SDL_JoystickGetButton(js, 10)
ps4.buttons.R3 = sdl2.SDL_JoystickGetButton(js, 11)
ps4.buttons.ps = sdl2.SDL_JoystickGetButton(js, 12)
ps4.buttons.pad = sdl2.SDL_JoystickGetButton(js, 13)
# get hat
# [up right down left] = [1 2 4 8]
ps4.buttons.hat = sdl2.SDL_JoystickGetHat(js, 0)
# print('b 12', sdl2.SDL_JoystickGetButton(js, 12))
# print('b 13', sdl2.SDL_JoystickGetButton(js, 13))
if verbose: print(Msg.Joystick.screen(ps4))
self.pub.pub('js', ps4)
time.sleep(dt)
except (IOError, EOFError):
print('[-] Connection gone .... bye')
break
# except Exception as e:
# print('Ooops:', e)
# else:
# raise Exception('Joystick: Something bad happened!')
# clean-up
sdl2.SDL_JoystickClose(js)
print('Bye ...')