import cozmo_fsm from cozmo_fsm import * from cozmo_fsm.worldmap import WallSpec from cozmo_fsm.worldmap import RoomObj import math class MakeWall(StateNode): def start(self,event=None): worldmap.wall_marker_dict.clear() # 1st floor front long wall no markers wall_A_spec = WallSpec(label='A', length=838.2, height=38.1) wall_A = WallObj(x=250, y=200, theta=0, wall_spec=wall_A_spec, is_fixed=True) worldmap.wall_marker_dict['Wall-A'] = wall_A_spec robot.world.world_map.add_fixed_landmark(wall_A) # 1st floor small wall no markers opp elevator side wall_B_spec = WallSpec(label='B', length=533.4, height=88.9) wall_B = WallObj(x = wall_A.x + wall_B_spec.length/2, y = wall_A.y+wall_A.length/2, theta = pi/2, wall_spec=wall_B_spec, is_fixed=True) worldmap.wall_marker_dict['Wall-B'] = wall_B_spec robot.world.world_map.add_fixed_landmark(wall_B) # 1st floor back long wall no markers wall_C_spec = WallSpec(label='C', length=838.2, height=232.8) wall_C = WallObj(x= wall_A.x+wall_B.length,y=wall_A.y, theta=0, is_fixed=True, wall_spec=wall_C_spec) worldmap.wall_marker_dict['Wall-C'] = wall_C_spec robot.world.world_map.add_fixed_landmark(wall_C) # 1st floor main large wall wall_1_spec = WallSpec(length=890.6, height=232.8, door_width=176, door_height=110, marker_specs={ 'Aruco-1' : (+1, ( 65.6, 50.)), 'Aruco-2' : (+1, (153.6,150.)), 'Aruco-3' : (+1, (241.6, 50.)), 'Aruco-4' : (+1, (357.3, 50.)), 'Aruco-5' : (+1, (445.3,150.)), 'Aruco-6' : (+1, (533.3, 50.)), 'Aruco-7' : (+1, (637.5, 50.)), 'Aruco-8' : (+1, (725.5, 150.)), 'Aruco-9' : (+1, (813.5, 50.)), 'Aruco-18' : (-1, ( 65.6, 50.)), 'Aruco-17' : (-1, (153.6,150.)), 'Aruco-16' : (-1, (241.6, 50.)), 'Aruco-15' : (-1, (357.3, 50.)), 'Aruco-14' : (-1, (445.3,150.)), 'Aruco-13' : (-1, (533.3, 50.)), 'Aruco-12' : (-1, (637.5, 50.)), 'Aruco-11' : (-1, (725.5, 150.)), 'Aruco-10' : (-1, (813.5, 50.)) }, doorways = [ (153.6, 176.), (445.3, 176.), (725.5, 176.) ], door_ids = [ 2, 5, 8, 11, 14, 17 ]) wall_1 = WallObj(x = wall_A.x+wall_B.length/2 , y = wall_A.y, theta =0, is_fixed=True, wall_spec = wall_1_spec) worldmap.wall_marker_dict[wall_1_spec.id] = wall_1_spec robot.world.world_map.add_fixed_landmark(wall_1) # first floor front small wall wall_55_spec = WallSpec(length=287.7, height=232.8, door_width=176, door_height=110, marker_specs={ 'Aruco-55' : (+1, ( 55.7, 50.)), 'Aruco-56' : (+1, (143.7,150.)), 'Aruco-57' : (+1, (231.7, 50.)), 'Aruco-60' : (-1, ( 55.7, 50.)), 'Aruco-59' : (-1, (143.7,150.)), 'Aruco-58' : (-1, (231.7, 50.)) }, doorways = [ (143.7, 176.) ], door_ids = [ 56, 59 ]) wall_55 = WallObj(x = wall_A.x+wall_55_spec.length/2, y = wall_A.y+wall_A.length/6, theta = pi/2, is_fixed=True, wall_spec=wall_55_spec) worldmap.wall_marker_dict[wall_55_spec.id] = wall_55_spec robot.world.world_map.add_fixed_landmark(wall_55) # first floor rear small wall wall_61_spec = WallSpec(length=287.7, height=232.8, door_width=176, door_height=110, marker_specs={ 'Aruco-61' : (+1, ( 55.7, 50.)), 'Aruco-62' : (+1, (143.7,150.)), 'Aruco-63' : (+1, (231.7, 50.)), 'Aruco-66' : (-1, ( 55.7, 50.)), 'Aruco-65' : (-1, (143.7,150.)), 'Aruco-64' : (-1, (231.7, 50.)) }, doorways = [ (143.7, 176.) ], door_ids = [ 62, 65 ]) wall_61 = WallObj(x = wall_A.x+(3*wall_B.length)/4, y = wall_A.y-wall_A.length/6, theta = pi/2, is_fixed=True, wall_spec=wall_61_spec) worldmap.wall_marker_dict[wall_61_spec.id] = wall_61_spec robot.world.world_map.add_fixed_landmark(wall_61) second_floor_x_shift = -700 second_floor_y_shift = 50 # 2nd floor front long wall no markers wall_D_spec = WallSpec(label='D', length=838.2, height=38.1) wall_D = WallObj(x = wall_A.x+second_floor_x_shift, y = wall_A.y+second_floor_y_shift, theta = 0, is_fixed=True, wall_spec=wall_D_spec) worldmap.wall_marker_dict['Wall-D'] = wall_D_spec robot.world.world_map.add_fixed_landmark(wall_D) # 2nd floor small wall no markers opp elevator side wall_E_spec = WallSpec(label='E', length=533.4, height=88.9) wall_E = WallObj(x = (wall_D.x + wall_E_spec.length/2), y = wall_D.y+wall_D.length/2, theta = pi/2, is_fixed=True, wall_spec=wall_E_spec) worldmap.wall_marker_dict['Wall-E'] = wall_E_spec robot.world.world_map.add_fixed_landmark(wall_E) # 2nd floor back long wall no markers wall_F_spec = WallSpec(label='F', length=838.2, height=232.8) wall_F = WallObj(x = wall_D.x+wall_E.length, y = wall_D.y, theta = 0, is_fixed=True, wall_spec=wall_F_spec) worldmap.wall_marker_dict['Wall-F'] = wall_F_spec robot.world.world_map.add_fixed_landmark(wall_F) # 2nd floor main large wall wall_19_spec = WallSpec(length=890.6, height=232.8, door_width=176, door_height=110, marker_specs={ 'Aruco-19' : (+1, ( 65.6, 50.)), 'Aruco-20' : (+1, (153.6,150.)), 'Aruco-21' : (+1, (241.6, 50.)), 'Aruco-22' : (+1, (357.3, 50.)), 'Aruco-23' : (+1, (445.3,150.)), 'Aruco-24' : (+1, (533.3, 50.)), 'Aruco-25' : (+1, (637.5, 50.)), 'Aruco-26' : (+1, (725.5, 150.)), 'Aruco-27' : (+1, (813.5, 50.)), 'Aruco-36' : (-1, ( 65.6, 50.)), 'Aruco-35' : (-1, (153.6,150.)), 'Aruco-34' : (-1, (241.6, 50.)), 'Aruco-33' : (-1, (357.3, 50.)), 'Aruco-32' : (-1, (445.3,150.)), 'Aruco-31' : (-1, (533.3, 50.)), 'Aruco-30' : (-1, (637.5, 50.)), 'Aruco-29' : (-1, (725.5, 150.)), 'Aruco-28' : (-1, (813.5, 50.)) }, doorways = [ (153.6, 176.), (445.3, 176.), (725.5, 176.) ], door_ids = [ 20, 23, 26, 29, 32, 35 ]) wall_19 = WallObj( x = (wall_D.x+wall_E.length/2), y = wall_D.y, theta=0, is_fixed=True, wall_spec=wall_19_spec) worldmap.wall_marker_dict[wall_19_spec.id] = wall_19_spec robot.world.world_map.add_fixed_landmark(wall_19) #second floor front small wall wall_67_spec = WallSpec(length=287.7, height=232.8, door_width=176, door_height=110, marker_specs={ 'Aruco-67' : (+1, ( 55.7, 50.)), 'Aruco-68' : (+1, (143.7,150.)), 'Aruco-69' : (+1, (231.7, 50.)), 'Aruco-72' : (-1, ( 55.7, 50.)), 'Aruco-71' : (-1, (143.7,150.)), 'Aruco-70' : (-1, (231.7, 50.)) }, doorways = [ (143.7, 176.) ], door_ids = [ 68, 71 ]) wall_67 = WallObj(x = (wall_D.x+wall_55_spec.length/2), y = wall_D.y+wall_D.length/6, theta = pi/2, is_fixed=True, wall_spec=wall_67_spec) worldmap.wall_marker_dict[wall_67_spec.id] = wall_67_spec robot.world.world_map.add_fixed_landmark(wall_67) # 2nd floor rear small wall wall_73_spec = WallSpec(length=287.7, height=232.8, door_width=176, door_height=110, marker_specs={ 'Aruco-73' : (+1, ( 55.7, 50.)), 'Aruco-74' : (+1, (143.7,150.)), 'Aruco-75' : (+1, (231.7, 50.)), 'Aruco-78' : (-1, ( 55.7, 50.)), 'Aruco-77' : (-1, (143.7,150.)), 'Aruco-76' : (-1, (231.7, 50.)) }, doorways = [ (143.7, 176.) ], door_ids = [ 74, 77 ]) wall_73 = WallObj(x = (wall_D.x+(3*wall_E.length)/4), y = wall_D.y-wall_D.length/6, theta = pi/2, is_fixed=True, wall_spec=wall_73_spec) worldmap.wall_marker_dict[wall_73_spec.id] = wall_73_spec robot.world.world_map.add_fixed_landmark(wall_73) """ third_floor_shift = -600 third_floor_y_shift =50 #third floor front long wall no markers wall_G_spec = WallSpec(label='G',length =838.2,height=38.1,door_width=0,door_height= 0,marker_specs=dict()) wall_G = WallObj(x= wall_D.x+third_floor_shift, y=wall_D.y+third_floor_y_shift, theta= 0, is_fixed = True, wall_spec= wall_G_spec) worldmap.wall_marker_dict['Wall-G'] = wall_G_spec robot.world.world_map.add_fixed_landmark(wall_G) #third floor small wall no markers opp elevator side wall_H_spec = WallSpec(label = 'H', marker_specs=dict(),length =533.4, height=88.9,door_width=0, door_height= 0) wall_H = WallObj(x= (wall_G.x + wall_H_spec.length/2), y= wall_G.y+wall_G.length/2, theta= pi/2, is_fixed =True, wall_spec = wall_H_spec) worldmap.wall_marker_dict['Wall-H'] = wall_H_spec robot.world.world_map.add_fixed_landmark(wall_H) #third floor back long wall no markers wall_I_spec = WallSpec(label ='I',marker_specs=dict(),length = 838.2, height= 232.8,door_width=0,door_height=0) wall_I = WallObj(x= wall_G.x+wall_H.length,y=wall_G.y, theta=0, is_fixed=True, wall_spec= wall_I_spec) worldmap.wall_marker_dict['Wall-I'] = wall_I_spec robot.world.world_map.add_fixed_landmark(wall_I) #third floor main large wall wall_37_spec = WallSpec(length=890.6, height=232.8, door_width = 76, door_height=110, marker_specs={ 37 : (+1, ( 65.6, 50.)), 38 : (+1, (153.6,150.)), 39 : (+1, (241.6, 50.)), 40 : (+1, (357.3, 50.)), 41 : (+1, (445.3,150.)), 42 : (+1, (533.3, 50.)), 43 : (+1, (637.5, 50.)), 44 : (+1, (725.5, 150.)), 45 : (+1, (813.5, 50.)), 54 : (-1, ( 65.6, 50.)), 53 : (-1, (153.6,150.)), 52 : (-1, (241.6, 50.)), 51 : (-1, (357.3, 50.)), 50 : (-1, (445.3,150.)), 49 : (-1, (533.3, 50.)), 48 : (-1, (637.5, 50.)), 47 : (-1, (725.5, 150.)), 46 : (-1, (813.5, 50.)) }, doorways = [ (153.6, 76.), (445.3, 76.), (725.5, 76.) ], door_ids = [ 38, 41, 44, 47, 50, 53 ]) wall_37 = WallObj(x=(wall_G.x+wall_H.length/2), y=wall_G.y, theta =0, is_fixed=True, wall_spec = wall_37_spec) worldmap.wall_marker_dict[wall_37_spec.id] = wall_37_spec robot.world.world_map.add_fixed_landmark(wall_37) # third floor front small wall wall_79_spec = WallSpec(length=287.7, height=232.8, door_width = 76, door_height=110, marker_specs={ 79 : (+1, ( 55.7, 50.)), 80 : (+1, (143.7,150.)), 81 : (+1, (231.7, 50.)), 84 : (-1, ( 55.7, 50.)), 83 : (-1, (143.7,150.)), 82 : (-1, (231.7, 50.))}, doorways = [ (143.7, 76.) ], door_ids = [ 80,83]) wall_79 = WallObj(x= (wall_G.x+wall_55_spec.length/2),y= wall_G.y+wall_G.length/6, theta =pi/2, is_fixed=True, wall_spec=wall_79_spec) worldmap.wall_marker_dict[wall_79_spec.id] = wall_79_spec robot.world.world_map.add_fixed_landmark(wall_79) #third floor rear small wall wall_85_spec = WallSpec(length=287.7, height=232.8, door_width = 76, door_height=110, marker_specs={ 85 : (+1, ( 55.7, 50.)), 86 : (+1, (143.7,150.)), 87 : (+1, (231.7, 50.)), 90 : (-1, ( 55.7, 50.)), 89 : (-1, (143.7,150.)), 88 : (-1, (231.7, 50.))}, doorways = [ (143.7, 76.) ], door_ids = [ 86,89]) wall_85 = WallObj(x= (wall_G.x+(3*wall_H.length)/4),y=wall_G.y-wall_G.length/6 , theta =pi/2, is_fixed = True, wall_spec=wall_85_spec) worldmap.wall_marker_dict[wall_85_spec.id] = wall_85_spec robot.world.world_map.add_fixed_landmark(wall_85) elevator_x_shift = 0 elevator_y_shift =-600 wall_J_spec = WallSpec(label ='J',marker_specs=dict(),length = 110.0, height= 165.1,door_width=0,door_height=0) wall_J = WallObj(x= wall_A.x+wall_B.length,y=wall_A.y-wall_A.length/2 -wall_J_spec.length/2, theta=0, is_fixed=True, wall_spec= wall_J_spec) worldmap.wall_marker_dict['Wall-J'] = wall_J_spec robot.world.world_map.add_fixed_landmark(wall_J) wall_91_spec = WallSpec(length=110, height=165.1, door_width = 50, door_height=100, marker_specs={ 91 : (+1, (3,76.2)), 92 : (+1, (57.1,91.4)), 93 : (+1, (111.3,76.2))}, doorways = [ (57.1,57.1) ], door_ids = [92]) wall_91 = WallObj(x=wall_J.x - wall_J.length/2, y=wall_J.y-wall_J.length/2 , theta =pi/2, is_fixed = True, wall_spec=wall_91_spec) worldmap.wall_marker_dict[wall_91_spec.id] = wall_91_spec robot.world.world_map.add_fixed_landmark(wall_91) wall_K_spec = WallSpec(label ='K',marker_specs=dict(),length = 110.0, height= 165.1,door_width=0,door_height=0) wall_K = WallObj(x= wall_J.x -wall_J.length,y=wall_J.y, theta=0, is_fixed=True, wall_spec= wall_K_spec) worldmap.wall_marker_dict['Wall-K'] = wall_K_spec robot.world.world_map.add_fixed_landmark(wall_K) """ super().start(event=None) self.post_completion() class MakeRooms(StateNode): def start(self, event=None): #first floor room_A = RoomObj('A', points = np.array([[0],[0],[0],[1]]), # FILL THIS IN floor=1, door_ids=['Doorway-5', 'Doorway-8', 'Doorway-56'], connections=[('Doorway-5','Room-D'), ('Doorway-8','Room-C'), ('Doorway-56','Room-B')]) self.robot.world.world_map.objects[room_A.id] = room_A """ room_B = RoomObj(id='B',width= world.world_map.objects['Wall-A'].length/3,height=232.8,floor=1,door_ids=[2,59],marker_specs=[1,2,3,58,59,60],list_of_dandr=[(2,'D'),(59,'A')]) robot.world.world_map.objects[room_B.id] = room_B room_C = RoomObj(id='C',width=world.world_map.objects['Wall-A'].length/3,height=232.8,floor=1,door_ids=[8,62],marker_specs=[10,11,12,61,62,63],list_of_dandr=[(8,'A'),(62,'D')],contains_elevator=True) robot.world.world_map.objects[room_C.id] = room_C room_D = RoomObj(id='D',width=world.world_map.objects['Wall-A'].length/3,height=232.8,floor=1,door_ids=[14,17,65],marker_specs=[13,14,15,16,17,18],list_of_dandr=[(14,'A'),(17,'B'),(65,'C')]) robot.world.world_map.objects[room_D.id] = room_D #second floor room_E = RoomObj(id='E',width=(2*world.world_map.objects['Wall-A'].length)/3,height=232.8,floor=2,door_ids=[23,26,68], marker_specs=[22,23,24,25,26,27,67.68,69], list_of_dandr=[(23,'H'),(26,'G'),(68,'F')]) robot.world.world_map.objects[room_E.id] = room_E room_F = RoomObj(id='F',width= world.world_map.objects['Wall-A'].length/3,height=232.8,floor=2,door_ids=[20,71],marker_specs=[19,20,21,70,71,72],list_of_dandr=[(20,'H'),(71,'E')]) robot.world.world_map.objects[room_F.id] = room_F room_G = RoomObj(id='G',width=world.world_map.objects['Wall-A'].length/3,height=232.8,floor=2,door_ids=[29,74],marker_specs=[28,29,30,73,74,75],list_of_dandr=[(29,'E'),(74,'H')],contains_elevator=True) robot.world.world_map.objects[room_G.id] = room_G room_H = RoomObj(id='H',width=(2*world.world_map.objects['Wall-A'].length)/3,height=232.8,floor=2,door_ids=[32,35,77],marker_specs=[31,32,33,34,35,36,76,77,78],list_of_dandr=[(32,'E'),(35,'F'),(77,'G')]) robot.world.world_map.objects[room_H.id] = room_H #third floor room_I = RoomObj(id='I',width=(2*world.world_map.objects['Wall-A'].length)/3,height=232.8,floor=3, door_ids=[41,44,80], marker_specs=[40,41,42,43,44,45,79,80,81], list_of_dandr=[(41,'L'),(44,'K'),(80,'J')]) robot.world.world_map.objects[room_I.id] = room_I room_J = RoomObj(id='J',width= world.world_map.objects['Wall-A'].length/3,height=232.8,floor=3,door_ids=[38,83],marker_specs=[82,83,84,37,38,39],list_of_dandr=[(38,'L'),(83,'I')]) robot.world.world_map.objects[room_J.id] = room_J room_K = RoomObj(id='K',width=world.world_map.objects['Wall-A'].length/3,height=232.8,floor=3,door_ids=[47,86],marker_specs=[46,47,48,85,86,87],list_of_dandr=[(47,'I'),(86,'L')],contains_elevator=True) robot.world.world_map.objects[room_K.id] = room_K room_L = RoomObj(id='L',width=(2*world.world_map.objects['Wall-A'].length)/3,height=232.8,floor=3,door_ids=[50,53,89],marker_specs=[49,50,51,52,53,54,88,89,90],list_of_dandr=[(50,'I'),(53,'J'),(89,'K')]) robot.world.world_map.objects[room_L.id] = room_L """ super().start(event=None) class DreamHouse(StateMachineProgram): def setup(self): """ MakeWall() =TM=> PilotToPose(Pose(-280, 270, 0, angle_z=degrees(math.nan))) =TM=> PilotToPose(Pose(0, 0, 0, angle_z=degrees(math.nan))) """ # Code generated by genfsm on Mon Apr 8 05:42:07 2019: makewall1 = MakeWall() .set_name("makewall1") .set_parent(self) pilottopose1 = PilotToPose(Pose(-280, 270, 0, angle_z=degrees(math.nan))) .set_name("pilottopose1") .set_parent(self) pilottopose2 = PilotToPose(Pose(0, 0, 0, angle_z=degrees(math.nan))) .set_name("pilottopose2") .set_parent(self) textmsgtrans1 = TextMsgTrans() .set_name("textmsgtrans1") textmsgtrans1 .add_sources(makewall1) .add_destinations(pilottopose1) textmsgtrans2 = TextMsgTrans() .set_name("textmsgtrans2") textmsgtrans2 .add_sources(pilottopose1) .add_destinations(pilottopose2) return self