def request_sensor_information(self, robot, filter, callback):
if not callable(callback):
print "Invalid callback!"
return
should_filter = filter != ""
filter_size = 0
if should_filter:
filter_size = 4 + len(filter)
total_size = 9 + len(robot) + filter_size
array = bytearray([0] * total_size)
buf = ByteBuffer(array, 0, total_size)
# 4 bytes
buf.put_SLInt32(total_size + 1)
# 4 + 4 = 8
buf.put_SLInt32(len(robot))
# 4 + 4 + 1 = 9
tmp = bytearray([0])
if should_filter:
tmp[0] = 1
buf.put_bytes(tmp, 0, 1)
temp2 = bytearray()
temp2.extend(robot)
buf.put_bytes(temp2, 0, len(temp2))
if should_filter:
buf.put_SLInt32(len(filter))
temp3 = bytearray()
temp3.extend(filter)
buf.put_bytes(temp3, 0, len(temp3))
to_send = bytearray([0] * (total_size + 1))
to_send[0] = 0x07
buf.set_position(0)
buf.get(to_send, 1, total_size)
self.socket.sendto(to_send, self.server_address)
if robot not in self.sensor_callbacks:
self.sensor_callbacks[robot] = list()
self.sensor_callbacks[robot].append(callback)
def body(dev, *args):
global alpha
global previousAcc
global velocity
global was_moving
acc = freenect.get_accel(dev)
accX = int(acc[0])
accY = int(acc[1])
accZ = int(acc[2])
accX = accX - previousAcc[0]
accY = accY - previousAcc[1]
accZ = accZ - previousAcc[2]
velocity[0] += accX
velocity[1] += accY
velocity[2] += accZ
previousAcc[0] = int(acc[0])
previousAcc[1] = int(acc[1])
previousAcc[2] = int(acc[2])
if abs(velocity[0]) > 0 or abs(velocity[2]) > 0:
print("Moving!!\tX: " + str(velocity[0]) + "m/s\tZ: " +
str(velocity[2]) + "m/s")
was_moving = True
elif was_moving:
print("Stopped moving")
was_moving = False
buf = ByteBuffer(bytearray([0] * 24), 0, 24)
buf.put_SLInt32(accX)
buf.put_SLInt32(accY)
buf.put_SLInt32(accZ)
buf.put_SLInt32(velocity[0])
buf.put_SLInt32(velocity[1])
buf.put_SLInt32(velocity[2])
arr = bytearray([0] * 25)
arr[0] = 0x03
buf.set_position(0)
buf.get(arr, 1, 24)
sock.send(arr)
if not keep_running:
raise freenect.Kill
def trigger_event(self, event_id, event_data, filter):
event = json.dumps(event_data)
should_filter = filter != ""
filter_size = 0
if should_filter:
filter_size = 4 + len(filter)
total_size = 10 + len(event) + filter_size
array = bytearray([0] * total_size)
buf = ByteBuffer(array, 0, total_size)
# 4 bytes
buf.put_SLInt32(total_size + 1)
# 4 + 2 = 6
temp = bytearray([0] * 2)
temp[0] = event_id
if should_filter:
temp[1] = 1
else:
temp[1] = 0
buf.put_bytes(temp, 0, 2)
# 4 + 2 + 4 = 10
buf.put_SLInt32(len(event))
temp2 = bytearray()
temp2.extend(event)
buf.put_bytes(temp2, 0, len(temp2))
if should_filter:
buf.put_SLInt32(len(filter))
temp3 = bytearray()
temp3.extend(filter)
buf.put_bytes(temp3, 0, len(temp3))
to_send = bytearray([0] * (total_size + 1))
to_send[0] = 0x06
buf.set_position(0)
buf.get(to_send, 1, total_size)
self.socket.sendto(to_send, self.server_address)
def send_robot_command(self, motor1, motor2, motor3, motor4, robot_name):
total_size = 24 + len(robot_name)
array = bytearray([0] * total_size)
buf = ByteBuffer(array, 0, total_size)
buf.put_SLInt32(total_size + 1)
buf.put_SLInt32(motor1)
buf.put_SLInt32(motor2)
buf.put_SLInt32(motor3)
buf.put_SLInt32(motor4)
buf.put_SLInt32(len(robot_name))
temp = bytearray()
temp.extend(robot_name)
buf.put_bytes(temp, 0, len(temp))
to_send = bytearray([0] * (total_size + 1))
to_send[0] = 0x08
buf.set_position(0)
buf.get(to_send, 1, total_size)
self.socket.sendto(to_send, self.server_address)