6003xuan
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

modify actor obstacle avoidance

This commit is contained in:
MALAN 2020-11-19 15:39:03 +08:00
parent cff930af11
commit 3f305263de
4 changed files with 1991 additions and 835 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
robocup/Openlist_py2.py Normal file
View File

@ -0,0 +1,54 @@
list_o = [[[-35.375, -34.825], [-4, -3]], [[-3.275, -2.725], [-4, -3]], [[20.725, 21.275], [-4, -3]], \
[[57.725, 58.275], [-4, -3]], [[83.725, 84.275], [-4, -3]], [[-25.475, -24.925], [3, 4]], [[8.725, 9.275], [3, 4]], \
[[26.725, 27.275], [3, 4]], [[70.725, 71.275], [3, 4]], [[90.225, 90.775], [3, 4]],[[52.2, 55.2],[-10.0, -5.72]], \
[[78, 91], [-34.6, -27]],[[2, 10], [33, 37]], [[-36.0, -19.0], [16.0, 34.5]], [[3.0, 23.0], [10.0, 26.0]], [[54, 70.0], [14, 31.3]], \
[[70.7, 84.5], [8.5, 19.5]], [[87.3, 101.7], [10.9, 17.2]], [[78, 95.7], [21.2, 35.5]], [[52, 71], [-36, -26]], [[-6.4, 6.5], [-20.4, -9.6]], \
[[13.5, 40.5], [-23.5, -4.5]], [[-6.0, 6.0], [-35.0, -21.0]], [[-28.8, -22.5], [-28.5, -14]], [[-37, -23], [-36, -30]], \
[[-36.5, -30.3], [-26.5, -12]], [[2.0,9.2],[33.3,36.7]]]
def change_list(list_o, d):
list_result = []
k = 1.0*d/2.0
count = 0
for item in list_o:
count += 1
y = item[1][0] + k
while (y+k) <= item[1][1]:
x = item[0][0]+k
if count == 1:
print y
#list_result.append([round(xx, 4),round(yy, 4)])
list_result.append([x, y])
while (x+k) <= item[0][1]:
x += d
#list_result.append([round(x, 4),round(yy, 4)])
list_result.append([x, y])
y += d
return list_result
# print change_list([[[15, 18], [-16, -12]]], 2)
result_list = change_list(list_o, 1.0)
length = len(result_list)
ARRS = []
f=open('obstacle.txt','w+')
for i in range(length):
jointsFrame = result_list[i]
ARRS.append(jointsFrame)
for Ji in range(2):
strNum = str(jointsFrame[Ji])
f.write(strNum)
f.write(' ')
f.write('\n')
f.close()

12
robocup/control_actor.py
View File

@ -52,13 +52,11 @@ class ControlActor:
self.catching_flag = 0 # if there is a uav tracking 'me' for a long time
self.catching_uav_num = 10 # get the number of uav of which is catching 'me'
#self.black_box = numpy.array([[[-34, -19], [16, 34]], [[5, 20], [10, 28]], [[53, 68], [13, 31]], [[70, 84], [8, 20]], [[86, 102], [10, 18]], [[77, 96], [22, 35]], [[52, 71], [-34, -25]], [[-6, 6], [-35, -20]], [[12, 40], [-20, -8]], [[-7, 8], [-21, -9]], [[-29, -22], [-16, -27]], [[-37, -30], [-27, -12]], [[-38, -24], [-36, -29]]])
self.black_box = [[[-35.375, -34.825], [-4, -3]], [[-3.275, -2.725], [-4, -3]], [[20.725, 21.275], [-4, -3]], \
[[57.725, 58.275], [-4, -3]], [[83.725, 84.275], [-4, -3]], [[-25.475, -24.925], [3, 4]], [[8.725, 9.275], [3, 4]], \
[[26.725, 27.275], [3, 4]], [[70.725, 71.275], [3, 4]], [[90.225, 90.775], [3, 4]],[[53.2, 54.2],[-9.0, -6.72]], \
[[79, 92], [-33.6, -28]],[[3, 8], [35, 36]], [[-34, -21.7], [18, 33]], [[4.5, 20.5], [11.5, 25]], [[55, 67.3], [15.5, 30.3]], \
[[71.7, 83.5], [9.5, 18.5]], [[88.3, 100.7], [11.9, 16.2]], [[79, 94.7], [22.2, 34.5]], [[53, 70], [-35, -27]], [[-5.4, 6.6], [-19.4, -10.6]], \
[[19.0, 38.5], [-22.0, -6.0]], [[-4.7, 4.4], [-33.4, -21.6]], [[-27.8, -23.5], [-27.5, -15]], [[-36, -24], [-35, -31]], \
[[-35.5, -31.3], [-25.5, -13]], [[3.0,8.2],[34.3,35.7]],[[14,17.5],[-15.7,-12.4]]]
self.black_box = [[[52.2, 55.2],[-10.0, -5.72]], \
[[78, 91], [-34.6, -27]],[[2, 10], [33, 37]], [[-36.0, -19.0], [16.0, 34.5]], [[3.0, 23.0], [10.0, 26.0]], [[54, 70.0], [14, 31.3]], \
[[70.7, 84.5], [8.5, 19.5]], [[87.3, 101.7], [10.9, 17.2]], [[78, 95.7], [21.2, 35.5]], [[52, 71], [-36, -26]], [[-6.4, 6.5], [-20.4, -9.6]], \
[[13.5, 40.5], [-23.5, -4.5]], [[-6.0, 6.0], [-35.0, -21.0]], [[-28.8, -22.5], [-28.5, -14]], [[-37, -23], [-36, -30]], \
[[-36.5, -30.3], [-26.5, -12]], [[2.0,9.2],[33.3,36.7]]]
self.box_num = len(self.black_box)
self.cmd_pub = rospy.Publisher('/actor_' + str(self.id) + '/cmd_motion', ActorMotion, queue_size=10)
#self.black_box = numpy.array(

2728
robocup/obstacle.txt
View File

File diff suppressed because it is too large Load Diff

32
robocup/score_cal.py
View File

@ -13,11 +13,11 @@ coordx_bias = 3
coordy_bias = 9
actor_id_dict = {'green':[0], 'blue':[1], 'brown':[2], 'white':[3], 'red':[4,5]}
def state_callback(msg, vehicle_id):
global fall_detect, start_time
if rospy.get_time() - start_time > 60 and msg.armed == 'False':
fall_detect[vehicle_id] = 1
print('UAV_' + str(vehicle_id) + ': falled')
# def state_callback(msg, vehicle_id):
# global fall_detect, start_time
# if rospy.get_time() - start_time > 60 and not msg.armed and fall_detect[vehicle_id] == 0:
# fall_detect[vehicle_id] = 1
# print('UAV_' + str(vehicle_id) + ': falled')
@ -40,22 +40,18 @@ def actor_info_callback(msg):
target_finish += 1
del_model('actor_'+str(i))
left_actors.remove(i)
time_usage = rospy.get_time() - start_time
if time_usage > 600:
print('score:',score)
print("Time out, mission failed")
print('actor_'+str(i)+' is OK')
print('Time usage:', time_usage)
# calculate score
if target_finish == 6:
score = (1200 - time_usage) - sensor_cost * 3e-3 - sum(fall_detect) * 30
score = (1200 - time_usage) - sensor_cost * 3e-3
print('score:',score)
print("Mission finished")
while True:
pass
else:
score = (2 + target_finish) * 60 - sensor_cost * 3e-3 - sum(fall_detect) * 30
score = (2 + target_finish) * 60 - sensor_cost * 3e-3
print('score:',score)
break
else:
@ -87,9 +83,9 @@ if __name__ == "__main__":
actor_brown_sub = rospy.Subscriber("/actor_brown_info",ActorInfo,actor_info_callback)
actor_red1_sub = rospy.Subscriber("/actor_red1_info",ActorInfo,actor_info_callback)
actor_red2_sub = rospy.Subscriber("/actor_red2_info",ActorInfo,actor_info_callback)
state_sub = [None] * 6
for vehicle_id in range(uav_num):
state_sub[vehicle_id] = rospy.Subscriber('/typhoon_h480_'+str(vehicle_id)+'/mavros/state', State, state_callback, vehicle_id)
# state_sub = [None] * 6
# for vehicle_id in range(uav_num):
# state_sub[vehicle_id] = rospy.Subscriber('/typhoon_h480_'+str(vehicle_id)+'/mavros/state', State, state_callback, vehicle_id)
# sensor cost
mono_cam = 1
@ -102,7 +98,7 @@ if __name__ == "__main__":
fall_detect = [0] * 6
target_finish = 0
score = (2 + target_finish) * 60 - sensor_cost * 3e-3 - sum(fall_detect) * 30
score = (2 + target_finish) * 60 - sensor_cost * 3e-3
rate = rospy.Rate(10)
while not rospy.is_shutdown():
@ -110,6 +106,12 @@ if __name__ == "__main__":
actors_pos_tmp = get_model_state('actor_' + str(i), 'ground_plane').pose.position
if not actors_pos_tmp.x**2+actors_pos_tmp.y**2 == 0:
actors_pos[i] = actors_pos_tmp
time_usage = rospy.get_time() - start_time
if time_usage > 600:
print('score:',score)
print("Time out, mission failed")
while True:
pass
score_pub.publish(score)
left_actors_pub.publish(str(left_actors))
rate.sleep()