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