-
Notifications
You must be signed in to change notification settings - Fork 0
/
test_moving_obs.py
58 lines (36 loc) · 1.3 KB
/
test_moving_obs.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
import rospy
import time
import numpy as np
from moving_obs import MovingObs
import matplotlib.pyplot as plt
moving_obs = MovingObs('/home/kai/catkin_ws/src/drproj/moving_obs.yaml',
'/home/kai/catkin_ws/src/drproj/empty.yaml',
'/home/kai/catkin_ws/src/drproj/empty.png')
rospy.init_node('test_moving_obs', anonymous=True)
# for key in moving_obs.config:
# print(moving_obs.config[key])
# print(moving_obs.interpolate_two_pts([0.0,0.0], [1.0,1.0]))
# traj_a = np.array(moving_obs.interpolate_multi_pts(moving_obs.config['a']))
# print(len(traj_a))
# plt.plot(traj_a[:,0], traj_a[:,1], 'ro')
# plt.show()
# traj_b = np.array(moving_obs.interpolate_multi_pts(moving_obs.config['b']))
# plt.plot(traj_b[:,0], traj_b[:,1])
# plt.show()
# moving_obs.refine()
# print(len(moving_obs.trajs))
# for traj in moving_obs.trajs:
# traj = np.array(traj)
# print(len(traj))
# plt.plot(traj[:,0], traj[:,1], 'ro')
# plt.show()
# moving_obs.grid_map.show('rviz_global_grid_map')
# time.sleep(3.0)
# moving_obs.run_once()
# moving_obs.grid_map.show('rviz_global_grid_map')
moving_obs.grid_map.show('rviz_global_grid_map')
while not rospy.core.is_shutdown():
time.sleep(0.1)
moving_obs.run_once()
moving_obs.grid_map.show('rviz_global_grid_map')
print('Finished')