def callback(data):
"""
param data: data from ROS joy topic
"""
global ID
global values
global old_vals
# fetch data from joy_events
data = json.loads(data.data)
# fetch joypad controller input
pan = data['Axes']['6'] # pan left/right
tilt = -data['Axes']['7'] # tilt down/up
# update values
if pan == -1:
values = [0xFF, 0] # [-1, 0]: pan left
elif pan == 1:
values = [0x01, 0] # [1, 0]: pan right
elif tilt == -1:
values = [0, 0xFF] # [0, -1]: tilt down
elif tilt == 1:
values = [0, 0x01] # [0, 1]: tilt up
else:
values = [0, 0] # [0, 0]: no movement
# send mast commands to CAN bus
if values != old_vals:
old_vals = values[:]
can_handler.send_msg(ID, values)
def callback(data):
"""
param data: data from ROS joy topic
"""
# rospy.loginfo(data) # debug
global ID
global values
global old_vals
global calib
# TODO: fix bug:
if data.axes[4] != 0 and calib == False:
calib = True
# fetch joypad controller input
drilling = math.fabs(data.axes[4] - 1)
mode = data.buttons[7]
# update values
if calib == True:
if mode == 0:
# clockwise
values[0] = 0
values[1] = int(drilling * ((2**8 - 1) / 2 + 1))
else:
# counter-clockwise
values[0] = 1
values[1] = int(drilling * ((2**8 - 1) / 2 + 1))
if values[1] > 255:
values[1] = 255
# send drill commands to CAN bus
if values != old_vals:
old_vals = values[:]
can_handler.send_msg(ID, values)
def callback(data):
"""
param data: data from ROS joy topic
"""
global ID
global values
global old_vals
# fetch joypad controller input
link_select = 0 # TODO
sway = -data.axes[2]
rise = data.axes[3]
if rise < 0:
rise = int(rise * ((2**16 - 1) / 2) - 1)
else:
rise = int(rise * ((2**16 - 1) / 2))
if sway < 0:
sway = int(sway * ((2**16 - 1) / 2) - 1)
else:
sway = int(sway * ((2**16 - 1) / 2))
r1, r2 = can_handler.split_bytes(rise, 2)
s1, s2 = can_handler.split_bytes(sway, 2)
values = [link_select, r1, r2, s1, s2]
# send manipulator commands to CAN bus
if values != old_vals:
old_vals = values[:]
can_handler.send_msg(ID, values)
def callback(data): """ param data: data from ROS joy topic """ global ID global values global old_vals # fetch joypad controller input pan = -data.axes[6] tilt = data.axes[7] # update values if pan == -1: values = [0xFF, 0] # [-1, 0]: pan left elif pan == 1: values = [0x01, 0] # [1, 0]: pan right elif tilt == -1: values = [0, 0xFF] # [0, -1]: tilt down elif tilt == 1: values = [0, 0x01] # [0, 1]: tilt up else: values = [0, 0] # [0, 0]: no movement # send mast commands to CAN bus if values != old_vals: old_vals = values[:] can_handler.send_msg(ID, values)
def callback(data):
"""
param data: data from ROS joy topic
"""
global ID
global values
global old_vals
# fetch joypad controller input
turning = data.axes[0]**3 # power of 3 to reduce sensitivity
speed = data.axes[1]
# convert raw data to uniform values wrt. linux character device values.
# ROS joy gives float values between -1 and 1
# speed:
if speed < 0:
raw_s = int(speed * ((2**16 - 1) / 2) - 1)
else:
raw_s = int(speed * ((2**16 - 1) / 2))
s_MSB, s_LSB = can_handler.split_bytes(raw_s, 2)
values[0] = int(s_MSB)
values[1] = int(s_LSB)
# turning:
if turning == 0:
# no turn, "infinite" turn radius (0x80000000 = -2**32 / 2)
values[2] = 0x80
values[3] = 0
values[4] = 0
values[5] = 0
else:
# using 32-bit value for turning radius
if turning < 0:
raw_tr = -turning * ((2**32 - 1) / 2)
tr = (2**32 - 1) / 2 - raw_tr # reverse axis
tr = int(math.sqrt(math.fabs(tr))) # reduce sensitivity
if tr == 0:
tr = 1 # turn right on own axis
else:
raw_tr = -turning * ((2**32 - 1) / 2) + 1
tr = (-2**32 - 1) / 2 - raw_tr # reverse axis
tr = -int(math.sqrt(math.fabs(tr))) # reduce sensitivity
if tr == 0:
tr = -1 # turn left on own axis
tr_B1, tr_B2, tr_B3, tr_B4 = can_handler.split_bytes(tr, 4)
values[2] = int(tr_B1)
values[3] = int(tr_B2)
values[4] = int(tr_B3)
values[5] = int(tr_B4)
# send wheel commands to CAN bus
if values != old_vals:
old_vals = values[:]
can_handler.send_msg(ID, values)
def callback(data):
"""
param data: data from joy_events topic
"""
global ID
global values
global old_vals
# fetch data from joy_events
data = json.loads(data.data)
# fetch joypad controller input
turning = data['Axes']['0']
speed = data['Axes']['1']
# update values
# speed:
s_MSB, s_LSB = can_handler.split_bytes(speed, 2)
values[0] = int(s_MSB)
values[1] = int(s_LSB)
# turning:
if turning == 0:
# no turn, "infinite" turn radius (0x80000000 = -2**32 / 2)
values[2] = 0x80
values[3] = 0
values[4] = 0
values[5] = 0
else:
# using 32-bit value for turning radius
if turning < 0:
raw_tr = turning * (2**16)
tr = (-2**32 - 1) / 2 + 1 - raw_tr # reverse axis
tr = -int(math.sqrt(math.fabs(tr))) # reduce sensitivity
if tr == 0:
tr = -1 # turn left on own axis
else:
raw_tr = (turning + 1) * (2**16) - 1
tr = (2**32 - 1) / 2 - raw_tr # reverse axis
tr = int(math.sqrt(math.fabs(tr))) # reduce sensitivity
if tr == 0:
tr = 1 # turn right on own axis
tr_B1, tr_B2, tr_B3, tr_B4 = can_handler.split_bytes(tr, 4)
values[2] = int(tr_B1)
values[3] = int(tr_B2)
values[4] = int(tr_B3)
values[5] = int(tr_B4)
# send wheel commands to CAN bus
if values != old_vals:
old_vals = values[:]
can_handler.send_msg(ID, values)
def callback(data):
"""
param data: data from ROS joy topic
"""
global ID
global values
global old_vals
# fetch data from joy_events
data = json.loads(data.data)
# fetch joypad controller input and update values
drilling = data['Axes']['5'] # drilling speed
direction = data['Buttons']['5'] # normal/reverse
values[0] = direction
values[1] = drilling
# send drill commands to CAN bus
if values != old_vals:
old_vals = values[:]
can_handler.send_msg(ID, values)
def callback(data):
"""
param data: data from ROS joy topic
"""
global ID
global values
global old_vals
# fetch joypad controller input and update values
direction = data.buttons[6]
digging = math.fabs(data.axes[5] - 1)
values[0] = direction
values[1] = int(digging * ((2**8 - 1) / 2 + 1))
if values[1] > 255:
values[1] = 255
# send digger commands to CAN bus
if values != old_vals:
old_vals = values[:]
can_handler.send_msg(ID, values)
def callback(data):
"""
param data: data from ROS joy topic
"""
global ID
global values
global old_vals
# fetch joypad controller input
option = data.buttons[11]
green = data.buttons[0]
red = data.buttons[1]
if option == 1:
if green == 1:
values = [0xFF] # full power
elif red == 1:
values = [0x00] # sleep mode
# send commands to CAN bus
if values != old_vals:
old_vals = values
can_handler.send_msg(ID, values)
def callback(data):
"""
param data: data from ROS joy topic
"""
global ID
global values
global old_vals
# fetch data from joy_events
data = json.loads(data.data)
# fetch joypad controller input
link_select = data['Buttons']['7']
rise = data['Axes']['3']
sway = data['Axes']['4']
r1, r2 = can_handler.split_bytes(rise, 2)
s1, s2 = can_handler.split_bytes(sway, 2)
values = [link_select, r1, r2, s1, s2]
# send manipulator commands to CAN bus
if values != old_vals:
old_vals = values[:]
can_handler.send_msg(ID, values)