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 __init__(self, net, topic='cmd'):
# mp.Process.__init__(self)
self.pub = zmq.Pub(net)
self.topic = topic
# init SDL2 and grab joystick
sdl2.SDL_Init(sdl2.SDL_INIT_JOYSTICK)
self.js = sdl2.SDL_JoystickOpen(0)
# grab info for display
a = sdl2.SDL_JoystickNumAxes(self.js)
b = sdl2.SDL_JoystickNumButtons(self.js)
h = sdl2.SDL_JoystickNumHats(self.js)
self.old_twist = Msg.Twist()
print('==========================================')
print(' Joystick ')
print(' axes:', a, 'buttons:', b, 'hats:', h)
print(' publishing: {}:{}'.format(net[0], net[1], topic))
print('==========================================')
def run(self):
pub = Zmq.Pub(self.addr, hwm=100)
twist = Msg.Twist()
# fd = sys.stdin.fileno()
# tty.setraw(sys.stdin.fileno())
while True:
# have to do some fancy stuff to avoid sending \n all the time
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(fd)
key = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
print('>>>', key)
if key == 'a':
twist.angular.z += 0.1
twist.angular.z = limit_max(twist.angular.z)
elif key == 'd':
twist.angular.z -= 0.1
twist.angular.z = limit_min(twist.angular.z)
elif key == 'w':
twist.linear.x += 0.1
twist.linear.x = limit_max(twist.linear.x)
elif key == 'x':
twist.linear.x -= 0.1
twist.linear.x = limit_min(twist.linear.x)
elif key == 's': # stop - all 0's
twist.linear.set(0.0, 0.0, 0.0)
twist.angular.set(0.0, 0.0, 0.0)
elif key == 'q':
exit()
pub.pub('twist_kb', twist)
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')