-
Notifications
You must be signed in to change notification settings - Fork 7
/
animation_3d.py
142 lines (115 loc) · 3.34 KB
/
animation_3d.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
import matplotlib
matplotlib.use('TKAgg')
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches
from matplotlib import animation
import random
import math
import pickle
from args import FLAGS
fig = plt.figure()
plt.axis('equal')
plt.grid()
ax = fig.add_subplot(111)
ax.set_xlim(-25, 125)
ax.set_ylim(-25, 125)
ax.set_xticks(np.arange(-25, 126, 25))
ax.set_yticks(np.arange(-25, 126, 25))
car = patches.Rectangle((0, 0), 0, 0, fc='y')
car.set_width(7.5)
car.set_height(7.5)
drone = patches.Rectangle((0, 0), 0, 0, fc='r')
drone.set_width(5)
drone.set_height(5)
current_x = 0
current_y = 0
current_x_drone = 0
current_y_drone = 0
drone_dir_x = 0
drone_dir_y = 0
current_x_car = 0
current_y_car = 0
car_dir_x = 0
car_dir_y = 0
################ adding obstacles
obstacle1 = patches.Rectangle((30, 0), 5, 50, fc='g')
obstacle2 = patches.Rectangle((20, 0), 5, 50, fc='g')
obstacle3 = patches.Rectangle((50, 0), 10, 50, fc='g')
def distance(x1, y1, x2, y2):
return math.sqrt((x1-x2)**2 + (y1-y2)**2)
def get_direction_given():
file = open("state_drone.txt", "r")
move_to = file.read().split()
if (len(move_to) > 0):
return float(move_to[0]), float(move_to[1]), float(move_to[2])
else:
return 0.0, 0.0, 0.0
def get_direction_given_car():
file = open("state_car.txt", "r")
move_to = file.read().split()
if (len(move_to) > 0):
return float(move_to[0]), float(move_to[1])
else:
return 0.0, 0.0
def move_drone(drone_dir_x, drone_dir_y, speed):
global current_x_drone
global current_y_drone
# here y represents height of drone (conventionally z)
if (distance(current_x_drone, current_y_drone, drone_dir_x, drone_dir_y) >= 1):
theta = math.atan2((drone_dir_y - current_y_drone) , (drone_dir_x - current_x_drone + 0.000001))
next_x = current_x_drone + speed*math.cos(theta)
next_y = current_y_drone + speed*math.sin(theta)
else:
next_x = current_x_drone
next_y = current_y_drone
drone.set_xy([next_x, next_y])
current_x_drone = next_x
current_y_drone = next_y
return drone
def move_car(car_dir_x, car_dir_y, speed):
global current_x_car
theta = math.atan2((car_dir_y - current_y_car) , (car_dir_x - current_x_car + 0.000001))
next_x = current_x_car + speed*math.cos(theta)
car.set_xy([next_x, 0])
current_x_car = next_x
return car
def init():
ax.add_patch(car)
ax.add_patch(drone)
ax.add_patch(obstacle1)
ax.add_patch(obstacle2)
ax.add_patch(obstacle3)
return car, drone,
def animate(i):
global move_dir_x_drone
global move_dir_y_drone
global move_dir_x_car
global move_dir_y_car
# for car
speed_car = 1.0
move_dir_x_car, move_dir_y_car = get_direction_given_car()
c = move_car(move_dir_x_car, move_dir_y_car, speed_car)
# for drone
speed_drone = 1.0
move_dir_x_drone, move_dir_y_drone, move_dir_z_drone = get_direction_given()
d = move_drone(move_dir_x_drone, move_dir_z_drone, speed_drone)
return c, d,
def main_animation():
# anim = animation.FuncAnimation(fig, animate,
# init_func=init,
# frames=1000,
# interval=500,
# blit=False)
anim = animation.FuncAnimation(fig, animate,
init_func=init,
frames=20,
interval=500,
blit=False)
#anim.save('movie.gif', writer='imagemagick')
plt.title('X-Z Plot')
plt.xlabel('X')
plt.ylabel('Z')
plt.show()
if (__name__ == '__main__'):
main_animation()