def test_ack(self):
ack = PacketACK.create(10)
buf = Packets.serialize(ack)
self.assertEqual(len(buf), 2)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.service_id, 10)
def test_nak(self):
nak = PacketNAK.create(127)
buf = Packets.serialize(nak)
self.assertEqual(len(buf), 2)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.service_id, 127)
def test_id(self):
exit = PacketID.create(3)
buf = Packets.serialize(exit)
self.assertEqual(len(buf), 2)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.device_id, 3)
def test_exit(self):
exit = PacketEXIT.create(0)
buf = Packets.serialize(exit)
self.assertEqual(len(buf), 2)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.service_id, 0)
def test_packet_key(self):
r_bytes = get_random_bytes(256)
key = PacketKEY.create(r_bytes)
buf = Packets.serialize(key)
self.assertEqual(len(buf), 257)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.symmetric_key, r_bytes)
def test_chall_resp(self):
r_bytes = get_random_bytes(256)
chall_resp = PacketCHALL_RESP.create(r_bytes)
buf = Packets.serialize(chall_resp)
self.assertEqual(len(buf), 257)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.encrypted_bytes, r_bytes)
def test_chall(self):
r_bytes = b'abcdefgh'
chall = PacketCHALL.create(r_bytes)
buf = Packets.serialize(chall)
self.assertEqual(len(buf), 9)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.random_bytes, r_bytes)
def test_set(self):
setp = PacketSET.create(8, 13.254)
buf = Packets.serialize(setp)
self.assertEqual(len(buf), 6)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.service_id,
setp.fields.service_id)
self.assertEqual(round(deserialized.fields.value, 5),
round(setp.fields.value, 5))
def test_val(self):
val = PacketVAL.create(8, 13.89, 1234467)
buf = Packets.serialize(val)
self.assertEqual(len(buf), 10)
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.service_id, val.fields.service_id)
self.assertEqual(round(deserialized.fields.value, 5),
round(val.fields.value, 5))
self.assertEqual(deserialized.fields.timestamp, val.fields.timestamp)
def __init__(self, addr, marker_id):
self.ADDR = addr
self.MARKER_ID = marker_id
self.dest_x, self.dest_y = 0, 0
self.center_x, self.center_y, self.head_x, self.head_y, self.theta = (
0, 0, 0, 0, 0)
self.pid = pid_gui.PID('ROBOT_' + str(marker_id))
self.packet = Packets()
self.reached = True
def test_desc(self):
name = b'abdefg'
unit = b'Cel'
desc = PacketDESC.create(2, name, unit, 0.0, 100.0)
buf = Packets.serialize(desc)
self.assertEqual(len(buf), 15 + len(name))
deserialized = Packets.deserialize(buf)
self.assertEqual(deserialized.fields.dev_class, 2)
self.assertEqual(deserialized.fields.unit, desc.fields.unit)
self.assertEqual(round(deserialized.fields.min_value, 5),
round(desc.fields.min_value, 5))
self.assertEqual(round(deserialized.fields.max_value, 5),
round(desc.fields.max_value, 5))
def __init__(self, addr, marker_id):
"""Creates an instance of Robot class
Arguments:
addr (str): 16 bit of the robots xbee
marker_id (int): id of aruco on the robot
"""
self.ADDR = addr
self.MARKER_ID = marker_id
self.destination = (0, 0)
self.center, self.head, self.theta = ((0, 0), (0, 0), 0)
self.pid = pid_gui.PID('ROBOT_' + str(marker_id))
self.packet = Packets()
self.needs_align = False
self.needs_to_move_forward = False
self.target_peice_num = None
self.target_peice_object = None
self.target_peice_position = None
self.target_peice_rotation = None
self.reached = True
self.selected_picking_destination = False
self.reached_picking_destination = False
self.selected_dropping_destination = False
self.reached_dropping_destination = False
self.needs_to_pick = False
self.needs_to_drop = False
self.stop_bit = 0
self.path = []
self.collision = False
self.dropping = False
self.blocked = False
def test_eot(self):
eot = PacketEOT.create()
buf = Packets.serialize(eot)
self.assertEqual(len(buf), 1)
class Robot(XBeeComm, Packets, pid_gui.PID):
"""A class to create multiple instances for different robots to hold the state and position of the robot
Attributes:
ADDR (str): holds the 16 bit xbee xbee on robot
MARKER_ID (int): id of the aruco on the robot
destination ((int, int)): holds the destination where the robot needs to go
center ((int, int)): holds the position of robot
theta (int): holds the error angle by with the robot needs to turn to align to destination (pid input for robot)
pid (pid_gui.PID class): a gui to tune the pid of the robot
packet (Packets class): to create data packets to share with the robot
Methods:
update_position(): updates the position of the robot (i.e center, head, theta)
get_position(): returns the the position of robot
set_destination(): sets the destination of the robot
send_packets(): sends packets to the robot
align_robots(): aligns the the robot to the given point to make it face it in the direction of the point
align_forward(): to maintain the robot at a particular distance from the obstacle
pick_block(): signals the robot to pick the block and rotate it by the specified angle
drop_block(): signals the robot to drop the block and rotate it by the specified angle
"""
def __init__(self, addr, marker_id):
"""Creates an instance of Robot class
Arguments:
addr (str): 16 bit of the robots xbee
marker_id (int): id of aruco on the robot
"""
self.ADDR = addr
self.MARKER_ID = marker_id
self.destination = (0, 0)
self.center, self.head, self.theta = ((0, 0), (0, 0), 0)
self.pid = pid_gui.PID('ROBOT_' + str(marker_id))
self.packet = Packets()
self.needs_align = False
self.needs_to_move_forward = False
self.target_peice_num = None
self.target_peice_object = None
self.target_peice_position = None
self.target_peice_rotation = None
self.reached = True
self.selected_picking_destination = False
self.reached_picking_destination = False
self.selected_dropping_destination = False
self.reached_dropping_destination = False
self.needs_to_pick = False
self.needs_to_drop = False
self.stop_bit = 0
self.path = []
self.collision = False
self.dropping = False
self.blocked = False
def update_position(self):
"""Updates the postion of the robot"""
try:
self.center, self.head, self.theta = aruco_lib.get_marker_angle(
frame, self.destination, self.MARKER_ID)
except Exception as e:
#print(e)
self.theta = 0
def get_position(self, mode="center"):
"""Returns the position of the robot
Arguments:
mode (str): "center" returns the cordinates of center of the robot, "head" returns the cordinates of head of the robot
Retruns:
((int, int)): position of robot
"""
if mode == "center":
return self.center
if mode == "head":
return self.head
def set_destination(self, pt):
"""Sets the destination of robot
Arguments:
pt ((int, int)): cordinates of the destination
"""
self.destination = pt
self.reached = False
def get_distance(self, pt, mode="center"):
"""Returns the distance of robot from given point
Arguments:
pt ((int, int)): cordinates of the points
Returns:
(float): distance from a point
"""
if mode == "center":
return distance(self.center, pt)
if mode == "head":
return distance(self.head, pt)
def send_packet(self, data):
"""Sends data packets to robot
Arguments:
data (tuple): a tuple containing the data to be sent to the robot
"""
self.packet.resetData()
#push data to be sent as packets to packets list
self.packet.push(data)
#create packet using the the data in packets list
data_packet = self.packet.createPacket()
#send packet to the robot
comm.tx(data_packet, self.ADDR)
def align_robot(self, pt):
"""Aligns the robot to face towards the given point
Arguments:
pt ((int, int)): cordinates of the point
Returns:
(bool): returns True if the robot is aligned else returns False
"""
#update the position of robot
self.update_position()
try:
#find the length of aruco
arucolength = distance(self.center, self.head)
#find the distance of the point from aruco center
center_dist = distance(self.center, pt)
#find the distance of point from the midpoint of one of the edges of the aruco
head_dist = distance(self.head, pt)
#find the slope of aruco
m2 = (self.center[1] - self.head[1]) / (self.center[0] -
self.head[0] + 0.001)
#find the slope of line connecting the aruco center and the point
m1 = (self.center[1] - pt[1]) / (self.center[0] - pt[0] + 0.001)
#find the angle between aruco and line joining the point and aruco center
err_angle = np.arctan((m2 - m1) /
(1 + m1 * m2) + 0.001) * 180 / np.pi
#find if the angle is obtuse
if head_dist > np.sqrt(arucolength**2 + center_dist**2):
#add 180 if the angle negative
if err_angle < 0:
err_angle = 180 + err_angle
#substract 180 if the angle is positive
elif err_angle > 0:
err_angle = -180 + err_angle
#if angle is 0
else:
err_angle = 180
if err_angle > 10:
self.send_packet(('Z', 1))
return False
elif err_angle < -10:
self.send_packet(('Z', 2))
return False
else:
self.send_packet(('Z', 0))
return True
except Exception as e:
print(e)
return False
return False
def align_forward(self):
"""Use this method to maintain a specified distance from the obstacle"""
#find the distance of robot from the target peice
dist = self.get_distance(self.target_peice_position)
#if the distance of the robot is more than acceptable move the robot forward
if dist > DISTANCE_FROM_BLOCK + 5:
self.send_packet(('F', 1))
return False
#if the distance of the robot is less than acceptable move the robot backward
elif dist < DISTANCE_FROM_BLOCK - 5:
self.send_packet(('F', 2))
return False
#if the distance of the robot is acceptable do nothing
else:
self.send_packet(('F', 0))
return True
def pick_block(self, angle):
"""Signals the Robot to pick up the block"""
self.send_packet(('B', 1, angle))
self.picked_block = True
def drop_block(self, angle=0):
"""Signals the Robot to drop the block"""
self.send_packet(('B', 2, angle))
self.picked_block = False
def move_towards_destination(self):
"""Sends data packets containing the error angle for pid and the pid tunning parameters"""
global frame
global blocks
global robots
self.stop_bit = 0
#if collision set stop bit to 1
if self.collision:
self.stop_bit = 1
#binary image consiting of obstacles with pixel values less than 127
self.blocks_map = cv2.resize(blocks.copy(),
downscale(ARENA_SIZE),
interpolation=cv2.INTER_CUBIC)
#get the pid tuinning values from the gui trackbar
self.pid.set_parameters()
self.update_position()
#find the distance of destination from center
head_dist = self.get_distance(self.destination, "center")
#if the distance is less than 10 than stop the robot and set self.reached to true
if head_dist <= 10:
self.reached = True
self.stop_bit = 1
else:
try:
#find the path from robot center to destination
self.path = astar(self.blocks_map, downscale(self.center),
downscale(self.destination))
#select the first element in the path list as the intermidiate destination
self.intermediate_y, self.intermediate_x = upscale(
self.path[1])
except Exception as e:
#if path not found and the destination near set the destination as intermidiate destination
if head_dist < 30:
self.intermediate_x, self.intermediate_y = self.destination
try:
self.center, self.head, self.theta = aruco_lib.get_marker_angle(
frame, (self.intermediate_x, self.intermediate_y),
self.MARKER_ID)
except Exception as e:
#print(e)
self.theta = 0
#empty packets list
self.packet.resetData()
#push the pid tunning values to the packets list
self.packet.push(('P', self.pid.kp, self.pid.ki, self.pid.kd))
#push the error angle to the packets list
self.packet.push(('A', self.theta + 180))
#push the stop bit to the packets list
self.packet.push(('S', self.stop_bit))
#create the data packet
data_packet = self.packet.createPacket()
#send the data packet to the robot
comm.tx(data_packet, self.ADDR)
def display(self):
"""Draws points and lines to indicate the position of robot"""
global frame
global block
cv2.line(frame, self.destination, self.center, (255, 255, 255), 1)
cv2.circle(frame, self.destination, 5, (0, 0, 255), thickness=-1)
cv2.circle(frame, self.center, 5, (0, 255, 0), thickness=-1)
try:
cv2.circle(frame, (self.intermediate_x, self.intermediate_y),
10, (0, 255, 255),
thickness=-1)
except Exception as e:
pass
#print(e)
cv2.circle(frame, self.head, 5, (0, 255, 0), thickness=-1)
class Robot(XBeeComm, Packets, pid_gui.PID):
def __init__(self, addr, marker_id):
self.ADDR = addr
self.MARKER_ID = marker_id
self.dest_x, self.dest_y = 0, 0
self.center_x, self.center_y, self.head_x, self.head_y, self.theta = (
0, 0, 0, 0, 0)
self.pid = pid_gui.PID('ROBOT_' + str(marker_id))
self.packet = Packets()
self.reached = True
def get_position(self):
return (self.center_x, self.center_y)
def set_destination(self, x, y):
self.dest_x, self.dest_y = x, y
self.reached = False
def get_distance(self, x, y):
return np.sqrt((self.center_x - x)**2 + (self.center_y - y)**2)
def send_info(self):
global frame
self.pid.set_parameters()
try:
self.center_x, self.center_y, self.head_x, self.head_y, self.theta = aruco_lib.get_marker_angle(
frame, (self.dest_x, self.dest_y), self.MARKER_ID)
except Exception as e:
self.theta = 0
#print(e)
self.packet.push(('T', self.dest_x, self.dest_y))
self.packet.push(('P', self.pid.kp, self.pid.ki, self.pid.kd))
self.packet.push(
('R', self.center_x, self.center_y, self.head_x, self.head_y))
self.packet.push(('A', self.theta + 180))
packet_data = self.packet.createPacket()
comm.tx(packet_data, self.ADDR)
cv2.line(frame, (self.dest_x, self.dest_y),
(self.center_x, self.center_y), (255, 255, 255), 1)
cv2.circle(frame, (self.dest_x, self.dest_y),
5, (0, 0, 255),
thickness=-1)
cv2.circle(frame, (self.center_x, self.center_y),
5, (0, 255, 0),
thickness=-1)
cv2.circle(frame, (self.head_x, self.head_y),
5, (0, 255, 0),
thickness=-1)
self.packet.resetData()
head_dist = self.get_distance(self.dest_x, self.dest_y)
if head_dist <= 20:
self.reached = True
