forked from xtdrone/XTDrone
modify actor control
This commit is contained in:
parent
9e481398bd
commit
81c33598fa
|
|
@ -1,8 +0,0 @@
|
|||
#!/bin/bash
|
||||
python control_actortest.py 0 &
|
||||
python control_actortest.py 1 &
|
||||
python control_actortest.py 2 &
|
||||
python control_actortest.py 3 &
|
||||
python control_actortest.py 4 &
|
||||
python control_actortest.py 5
|
||||
|
||||
|
|
@ -1,169 +0,0 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import rospy
|
||||
import math
|
||||
from geometry_msgs.msg import Point
|
||||
from std_msgs.msg import String
|
||||
from gazebo_msgs.srv import DeleteModel,GetModelState
|
||||
|
||||
NODE_NAME = 'del_actor'
|
||||
|
||||
DAMAGE_RANGE = 15
|
||||
MAX_LIFE_VALUE = 20
|
||||
flag = [False for i in range(6)]
|
||||
delta_time = [0 for i in range(6)]
|
||||
judge_time = [0 for i in range(6)]
|
||||
firsttime = [0 for i in range(6)]
|
||||
actor_pos = [Point() for i in range(6)]
|
||||
actor_real_pos = [Point() for i in range(6)]
|
||||
delta_pose = [0.0 for i in range(6)]
|
||||
#an uav can cause 2 damge value every second
|
||||
CHECK_RATE = 1
|
||||
exchange_flag = False
|
||||
judge_flag = [False for i in range(6)]
|
||||
file = open ('result2.txt','w')
|
||||
'''
|
||||
Here to set the vehicle name and number of actor and uav
|
||||
'''
|
||||
actor_num = 6
|
||||
uav_num = 6
|
||||
VEHICLE_NAME = 'typhoon_h480'
|
||||
|
||||
|
||||
rospy.init_node(NODE_NAME)
|
||||
|
||||
rate = rospy.Rate(CHECK_RATE)
|
||||
count = 0
|
||||
|
||||
def string2float(i):
|
||||
return float(i)
|
||||
|
||||
def judge_callback(msg):
|
||||
string_msg = msg.data
|
||||
strlist = string_msg.split(' ')
|
||||
id = int(strlist[0])
|
||||
actor_pos[id].x = string2float(strlist[1])
|
||||
actor_pos[id].y = string2float(strlist[2])
|
||||
actor_pos[id].z = 0.0
|
||||
judge_flag[id] = True
|
||||
if not flag[id]:
|
||||
firsttime[id] = rospy.get_time()
|
||||
flag[id] = True
|
||||
else:
|
||||
delta_time[id] = rospy.get_time()-firsttime[id]
|
||||
|
||||
'''
|
||||
ros surbcriber
|
||||
'''
|
||||
judger_sub = rospy.Subscriber('judger',String,judge_callback)
|
||||
'''
|
||||
publisher
|
||||
'''
|
||||
human_disappear_pub = rospy.Publisher('judger_react',String,queue_size = 10)
|
||||
|
||||
|
||||
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
del_model = rospy.ServiceProxy(
|
||||
"/gazebo/delete_model",DeleteModel
|
||||
)
|
||||
|
||||
get_model_state = rospy.ServiceProxy(
|
||||
"/gazebo/get_model_state",GetModelState
|
||||
)
|
||||
|
||||
life_value_list = [MAX_LIFE_VALUE]*actor_num
|
||||
def VectNorm2(Pt1, Pt2):
|
||||
norm = math.sqrt(pow(Pt1.x - Pt2.x, 2) + pow(Pt1.y - Pt2.y, 2))
|
||||
return norm
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
count += 1
|
||||
exchange_flag = False
|
||||
for actor_id in range(actor_num):
|
||||
try:
|
||||
get_actor_state = get_model_state('actor_' + str(actor_id), 'ground_plane')
|
||||
actor_real_pos[actor_id] = get_actor_state.pose.position
|
||||
except rospy.ServiceException as e:
|
||||
print("Gazebo model state service"+str(actor_id)+" call failed: %s") % e
|
||||
actor_real_pos[actor_id] = 0.0
|
||||
actor_real_pos[actor_id] = 0.0
|
||||
actor_real_pos[actor_id] = 1.25
|
||||
if firsttime[actor_id] > 0:
|
||||
print 'start !!!!!'
|
||||
judge_time[actor_id] = rospy.get_time()-firsttime[actor_id]
|
||||
if judge_flag[actor_id]:
|
||||
judge_flag[actor_id] = False
|
||||
delta_pose[actor_id] = VectNorm2(actor_real_pos[actor_id],actor_pos[actor_id])
|
||||
if actor_id == 0:
|
||||
actor_exchange_id = 5
|
||||
if delta_pose[actor_id] > 10.0:
|
||||
try:
|
||||
get_actor_state = get_model_state('actor_' + str(actor_exchange_id), 'ground_plane')
|
||||
actor_real_pos[actor_exchange_id] = get_actor_state.pose.position
|
||||
delta_pose[actor_exchange_id] = VectNorm2(actor_real_pos[actor_exchange_id],actor_pos[actor_id])
|
||||
except rospy.ServiceException as e:
|
||||
print("Gazebo model state service"+str(actor_exchange_id)+" call failed: %s") % e
|
||||
actor_real_pos[actor_exchange_id] = 0.0
|
||||
actor_real_pos[actor_exchange_id] = 0.0
|
||||
actor_real_pos[actor_exchange_id] = 1.25
|
||||
if delta_pose[actor_exchange_id] < delta_pose[actor_id]:
|
||||
exchange_flag = True
|
||||
if not exchange_flag:
|
||||
file.write(str(actor_id)+':')
|
||||
file.write('\n')
|
||||
file.write(str(actor_real_pos[actor_id]))
|
||||
file.write('\n')
|
||||
file.write(str(actor_pos[actor_id]))
|
||||
file.write('\n')
|
||||
file.write(str(delta_pose[actor_id]))
|
||||
file.write('\n')
|
||||
else:
|
||||
file.write(str(actor_exchange_id)+':')
|
||||
file.write('\n')
|
||||
file.write(str(actor_real_pos[actor_exchange_id]))
|
||||
file.write('\n')
|
||||
file.write(str(actor_pos[actor_exchange_id]))
|
||||
file.write('\n')
|
||||
file.write(str(delta_pose[actor_exchange_id]))
|
||||
file.write('\n')
|
||||
file.write('\n')
|
||||
if delta_time[actor_id] >=20:
|
||||
if exchange_flag:
|
||||
exchange_flag = False
|
||||
del_model( 'actor_'+str(actor_exchange_id) )
|
||||
print('actor_'+str(actor_exchange_id)+' was killed')
|
||||
else:
|
||||
del_model( 'actor_'+str(actor_id) )
|
||||
print('actor_'+str(actor_id)+' was killed')
|
||||
delta_time[actor_id] = 0
|
||||
firsttime[actor_id] = 0
|
||||
judge_time[actor_id] = 0
|
||||
human_disappear_pub.publish(str(actor_id))
|
||||
|
||||
if judge_time[actor_id] > 25:
|
||||
delta_time[actor_id] = 0
|
||||
firsttime[actor_id] = 0
|
||||
judge_time[actor_id] = 0
|
||||
|
||||
#display the life value of all actors
|
||||
#for actor_id in range(actor_num):
|
||||
#print("actor_"+str(actor_id)+f"::{life_value_list[actor_id]}", end=" ")
|
||||
#print(" ")
|
||||
rate.sleep()
|
||||
file.close()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#!/bin/bash
|
||||
python control_actor.py 0 &
|
||||
python control_actor.py 1 &
|
||||
python control_actor.py 2 &
|
||||
python control_actor.py 3 &
|
||||
python control_actor.py 4 &
|
||||
python control_actor.py 5
|
||||
|
||||
|
|
@ -0,0 +1 @@
|
|||
kill -9 $(ps -ef|grep control_actor.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
|
||||
Loading…
Reference in New Issue