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

modified actor control

This commit is contained in:
Your Name 2020-11-12 15:09:35 +08:00
parent b3a4c9e41a
commit e048b84b6c
2 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin_msgs/msg/ActorInfo.msg
View File

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

47
robocup/score_cal.py
View File

@ -4,37 +4,40 @@ from std_msgs.msg import String,Int16
from ros_actor_cmd_pose_plugin_msgs.msg import ActorInfo
from gazebo_msgs.srv import DeleteModel,GetModelState
actor_num = 6
err_threshold = 1
coordx_bias = 3
coordy_bias = 9
actor_id_dict = {'green':[0], 'blue':[1], 'brown':[2], 'white':[3], 'red':[4,5]}
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)
actor_id = actor_id_dict[msg.cls]
for i in actor_id:
actor_pos = get_model_state('actor_' + str(actor_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)
# 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:
count_flag[msg.id] = False
# 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:
count_flag[msg.id] = False
if __name__ == "__main__":
actor_num = 6
left_actors = range(actor_num)
count_flag = [False] * actor_num
rospy.init_node('score_cal')