jasder
/
XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

modify actor control

This commit is contained in:
Your Name 2020-11-12 14:53:20 +08:00
parent 25b33f5137
commit b3a4c9e41a
7 changed files with 88 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
control/actor/control_actortest.py
View File

@ -107,7 +107,6 @@ class ControlActor:
get_actor_state = self.gazeboModelstate('actor_' + self.id, 'ground_plane')
self.last_pose = self.current_pose
self.gazebo_actor_pose = get_actor_state.pose.position
self.current_vel = get_actor_state.twist.linear
self.current_pose = self.gazebo_actor_pose
except rospy.ServiceException as e:
print("Gazebo model state service"+str(self.id)+" call failed: %s") % e
@ -157,8 +156,8 @@ class ControlActor:
self.target_pose.y = copy.deepcopy(middd_pos[0].y)
#self.avoid_start_flag = True
# print 'current_position: ' + str(self.id)+' ', self.current_pose
print 'middd_pos: '+ str(self.id)+' ', middd_pos
print '\n'
# print 'middd_pos: '+ str(self.id)+' ', middd_pos
# print '\n'
except:
dis_list = [0,0,0,0]
for i in range(len(self.black_box)):
@ -192,8 +191,8 @@ class ControlActor:
if self.arrive_count > 5:
self.distance_flag = True
self.arrive_count = 0
if self.catching_flag == 1:
self.catching_flag = 2
if self.catching_flag == 2:
self.catching_flag = 0
else:
self.distance_flag = False
else:
@ -222,9 +221,8 @@ class ControlActor:
# # escaping (get a new target position)
if self.catching_flag == 1:
self.catching_flag = 2
flag_k = 0
angle = self.pos2ang(self.current_vel.x, self.current_vel.y)
angle = self.pos2ang(self.gazebo_uav_twist[self.catching_uav_num].x,self.gazebo_uav_twist[self.catching_uav_num].y)
print 'angle: ', angle
tar_angle = angle - math.pi/2 # escape to the target vertical of the uav
if tar_angle == math.pi/2:
@ -235,57 +233,60 @@ class ControlActor:
k = math.tan(tar_angle)
print 'k: ', k
if (tar_angle < 0) and (flag_k == 0):
y = k*(self.x_max-self.current_pose.x)+self.current_pose.y
y = k*(self.x_max-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y
if y < self.y_min:
self.target_pose.y = self.y_min
self.target_pose.x = (self.y_min - self.current_pose.y) / k + self.current_pose.x
self.target_pose.x = (self.y_min - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x
else:
self.target_pose.y = y
self.target_pose.x = self.x_max
elif (tar_angle > 0) and (tar_angle < math.pi/2) and (flag_k == 0):
y = k*(self.x_max-self.current_pose.x)+self.current_pose.y
y = k*(self.x_max-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y
if y > self.y_max:
self.target_pose.y = self.y_max
self.target_pose.x = (self.y_max - self.current_pose.y) / k + self.current_pose.x
self.target_pose.x = (self.y_max - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x
else:
self.target_pose.y = y
self.target_pose.x = self.x_max
elif (tar_angle < math.pi) and (tar_angle > math.pi/2) and (flag_k == 0):
y = k*(self.x_min-self.current_pose.x)+self.current_pose.y
y = k*(self.x_min-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y
if y > self.y_max:
self.target_pose.y = self.y_max
self.target_pose.x = (self.y_max - self.current_pose.y) / k + self.current_pose.x
self.target_pose.x = (self.y_max - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x
else:
self.target_pose.y = y
self.target_pose.x = self.x_min
elif (tar_angle > math.pi) and (tar_angle < 3*math.pi/2) and (flag_k == 0):
y = k*(self.x_min-self.current_pose.x)+self.current_pose.y
y = k*(self.x_min-self.gazebo_uav_pose[self.catching_uav_num].x)+self.gazebo_uav_pose[self.catching_uav_num].y
if y < self.y_min:
self.target_pose.y = self.y_min
self.target_pose.x = (self.y_min - self.current_pose.y) / k + self.current_pose.x
self.target_pose.x = (self.y_min - self.gazebo_uav_pose[self.catching_uav_num].y) / k + self.gazebo_uav_pose[self.catching_uav_num].x
else:
self.target_pose.y = y
self.target_pose.x = self.x_min
elif flag_k == 1:
self.target_pose.x = self.current_pose.x
self.target_pose.x = self.gazebo_uav_pose[self.catching_uav_num].x
self.target_pose.y = self.y_max
elif flag_k == 2:
self.target_pose.x = self.current_pose.x
self.target_pose.x = self.gazebo_uav_pose[self.catching_uav_num].x
self.target_pose.y = self.y_min
print str(self.id) + ' self.curr_pose:', self.current_pose
print str(self.id) + ' self.curr_pose:', self.gazebo_uav_pose[self.catching_uav_num]
print str(self.id) + ' self.target_pose:', self.target_pose
print 'escaping change position'
self.subtarget_pos = self.Obstacleavoid.GetPointList(self.current_pose, self.target_pose, 1) # current pose, target pose, safe distance
self.subtarget_length = 1
# middd_pos = [Point() for k in range(self.subtarget_length)]
# middd_pos = copy.deepcopy(self.subtarget_pos)
self.target_pose.x = self.subtarget_pos[0].x
self.target_pose.y = self.subtarget_pos[0].y
#self.avoid_start_flag = True
# print 'current_position: ' + str(self.id)+' ', self.current_pose
print 'target_pose: '+ str(self.id)+' ', self.target_pose
print '\n'
#self.avoid_finish_flag = False
try:
print str(self.id)+'general change position'
self.subtarget_pos = self.Obstacleavoid.GetPointList(self.current_pose, self.target_pose, 1) # current pose, target pose, safe distance
self.subtarget_length = 1
# middd_pos = [Point() for k in range(self.subtarget_length)]
# middd_pos = copy.deepcopy(self.subtarget_pos)
self.target_pose.x = self.subtarget_pos[0].x
self.target_pose.y = self.subtarget_pos[0].y
self.catching_flag = 2
except:
self.target_pose.x = self.target_pose.x
self.target_pose.y = self.target_pose.y
self.catching_flag = 1
# check if the actor is shot:
if self.get_moving:
@ -316,10 +317,10 @@ class ControlActor:
self.target_pose.x = middd_pos[self.subtarget_count].x
self.target_pose.y = middd_pos[self.subtarget_count].y
if self.catching_flag == 1:
self.target_pose.z = 3.0
if self.catching_flag == 1 or self.catching_flag == 2:
self.target_pose.z = 3
else:
self.target_pose.z = 2.0
self.target_pose.z = 2
if self.count % 200 == 0:
print str(self.id) + ' vel:', self.target_pose.z
self.cmd_pub.publish(self.target_pose)

0
control/actor/delet_actor_demo_old.py → control/actor/delet_actor_demo.py
View File

1
control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin_msgs/CMakeLists.txt
View File

@ -9,6 +9,7 @@ find_package(catkin REQUIRED
add_message_files(
FILES
ActorInfo.msg
ActorMotion.msg
)

3
control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin_msgs/msg/ActorInfo.msg Normal file
View File

@ -0,0 +1,3 @@
string class
float32 x
float32 y

0
control/actor/delet_actor_demo_ml.py → robocup/backup.py
View File

8
robocup/msg/judge.msg
View File

@ -1,8 +0,0 @@
string actor_class
int64 uav_id
float32 actor_pos_x
float32 actor_pos_y
float32 box_xmin
float32 box_xmax
float32 box_ymin
float32 box_ymax

172
robocup/score_cal.py
View File

@ -1,130 +1,53 @@
import rospy
import math
from geometry_msgs.msg import Point
from std_msgs.msg import String
import sys
from std_msgs.msg import String,Int16
from ros_actor_cmd_pose_plugin_msgs.msg import ActorInfo
from gazebo_msgs.srv import DeleteModel,GetModelState
NODE_NAME = 'score_cal'
err_threshold = 1
coordx_bias = 3
coordy_bias = 9
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
actor_finish = []
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'
def actor_info_callback(msg):
global target_finish, start_time, time_usage, score, count_flag
actor_pos = get_model_state('actor_' + str(msg.id), 'ground_plane').pose.position
if (msg.x-actor_pos.x+coordx_bias)**2+(msg.y-actor_pos.y+coordy_bias)**2<err_threshold**2:
if not count_flag[msg.id]:
count_flag[msg.id] = True
find_time = rospy.Time.now().secs
elif rospy.Time.now().secs - find_time >= 15:
target_finish += 1
del_model('actor_'+str(msg.id))
time_usage = rospy.Time.now().secs - start_time
print('actor_'+str(msg.id)+'is OK')
print('Time usage:', time_usage)
rospy.init_node(NODE_NAME)
rate = rospy.Rate(CHECK_RATE)
count = 0
def judge_callback(msg):
string_msg = msg.data
strlist = string_msg.split(' ')
id = int(strlist[0])
actor_pos[id].x = float(strlist[1])
actor_pos[id].y = float(strlist[2])
actor_pos[id].z = 0.0
exchange_flag = False
if id == 0:
if not id in actor_finish:
delta_pose[id] = VectNorm2(actor_real_pos[id],actor_pos[id])
else:
exchange_flag = True
if delta_pose[id] > 20.0 or exchange_flag:
if not 5 in actor_finish:
delta_pose[5] = VectNorm2(actor_real_pos[5],actor_pos[id])
if delta_pose[5] < delta_pose[id]:
id = 5
actor_pos[id].x = float(strlist[1])
actor_pos[id].y = float(strlist[2])
actor_pos[id].z = 0.0
judge_flag[id] = True
if not flag[id]:
firsttime[id] = rospy.get_time()
flag[id] = True
# calculate score
if target_finish == 6:
score = (800 - time_usage) - sensor_cost * 3e-3
print("Mission finished")
sys.exit()
else:
score = (1 + target_finish) * 2e2 - sensor_cost * 3e-3
print('score:',score)
else:
delta_time[id] = rospy.get_time()-firsttime[id]
count_flag[msg.id] = False
if __name__ == "__main__":
judger_sub = rospy.Subscriber('judger',String,judge_callback)
human_disappear_pub = rospy.Publisher('judger_react',String,queue_size = 10)
actor_num = 6
left_actors = range(actor_num)
count_flag = [False] * actor_num
rospy.init_node('score_cal')
del_model = rospy.ServiceProxy("/gazebo/delete_model",DeleteModel)
get_model_state = rospy.ServiceProxy("/gazebo/get_model_state",GetModelState)
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
score_pub = rospy.Publisher("score",Int16,queue_size=2)
left_actors_pub = rospy.Publisher("left_actors",String,queue_size=2)
actor_info_sub = [None] * actor_num
for actor_id in range(actor_num):
actor_info_sub = rospy.Subscriber("/actor_"+str(actor_id)+"_info",ActorInfo,actor_info_callback)
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 not actor_id in actor_finish:
actor_finish.append(actor_id)
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])
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')
file.write('\n')
if delta_time[actor_id] >=20:
if not actor_id in actor_finish:
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
flag[actor_id] = False
#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()
# 传感器费用
# sensor cost
mono_cam = 1
stereo_cam =0
laser1d = 0
@ -132,10 +55,17 @@ if __name__ == "__main__":
laser3d = 0
gimbal = 1
sensor_cost = mono_cam * 5e2 + stereo_cam * 1e3 + laser1d * 2e2+ laser2d * 5e3 + laser3d * 2e4 + gimbal * 2e2
target_finish = 0
time_usage = 10
score = (1 + target_finish) * 2e2 - sensor_cost * 3e-3
rate = rospy.Rate(10)
start_time = rospy.Time.now().secs
while not rospy.is_shutdown():
score_pub.publish(score)
rate.sleep()
# 计算得分
if success:
score = (800 - time_usage) - sensor_cost * 3e-3
else:
score = (1 + target_finish) * 2e2 - sensor_cost * 3e-3