def test_logic_sim():
target_wp1 = Pos(1.1, 2.2, 3.3)
target_wp2 = Pos(10.1, 20.2, 30.3)
target_wp3 = Pos(20.1, 10.2, 0.0)
sensor_drone = SensorDrone('SensorDrone', target_wp1)
suicide_drone = SuicideDrone('Suicide', target_wp2)
ugv = Ugv('UGV', target_wp3)
ls = LogicSim({suicide_drone.id: suicide_drone, sensor_drone.id:sensor_drone, ugv.id:ugv} )
for _ in range(1000):
if random.random() > 0.9:
target_wp1 = get_new_target(target_wp1)
target_wp2 = get_new_target(target_wp2)
target_wp3 = get_new_target(target_wp3)
assert not target_wp1 is None
assert not target_wp2 is None
assert not target_wp3 is None
# actions = {11:target_wp1, 22:target_wp2}
actions = {'MOVE_TO':[{'SensorDrone': (target_wp1)},{'Suicide': (target_wp2)}],
'LOOK_AT':[{'SensorDrone': (target_wp3)}],
'ATTACK':[],
'TAKE_PATH':[{'UGV':('Path1',target_wp3)}]}
# actions = {11:target_wp1}
ls.step(actions)
str_actions = [str(k)+','+str(v) for k,v in actions.items()]
print(str_actions)
print(ls)
def sim_ent(entity):
# str_2_type = {'Suicide':SuicideDrone, 'SensorDrone':SensorDrone, 'UGV':Ugv}
if entity.id == 'Suicide':
return lg_scn_drone(entity.id, Pos(entity.gpoint.x, entity.gpoint.y, entity.gpoint.z))
if entity.id == 'SensorDrone':
return lg_scd_drone(entity.id, Pos(entity.gpoint.x, entity.gpoint.y, entity.gpoint.z))
if entity.id == 'UGV':
return lg_ugv(entity.id, Pos(entity.gpoint.x, entity.gpoint.y, entity.gpoint.z))
def ambush_on_indication_target(action_list, log_drn, log_scd, log_ugv, sniper,
plan_index, start_ambush_step, step,
stimulation_1_step, stimulation_2_step,
gate_pos_commanded, move_commanded,
attack2_commanded, episode_experience):
if start_ambush_step == 0:
start_ambush_step = step
logging.info(
'step {} all entities positioned... start ambush phase'.format(
step))
if start_ambush_step + TIME_TO_STIMULATE_1 < step < start_ambush_step + TIME_TO_STIMULATE_2:
# STIMULATION 1
if stimulation_1_step == 0:
stimulation_1_step = step
logging.info('step {} stimulation 1 phase'.format(step))
# ugv.attack(WEST_WINDOW_POS)
add_action(action_list, log_ugv, 'ATTACK', (WEST_WINDOW_POS, ))
elif step > start_ambush_step + TIME_TO_STIMULATE_2:
# STIMULATION 2
if stimulation_2_step == 0:
stimulation_2_step = step
logging.info('step {} stimulation 2 phase'.format(step))
if not attack2_commanded and is_entity_positioned(log_ugv, GATE_POS):
# ugv.attack(WEST_WINDOW_POS)
add_action(action_list, log_ugv, 'ATTACK', (WEST_WINDOW_POS, ))
attack2_commanded = True
else:
if not gate_pos_commanded:
add_action(action_list, log_ugv, 'TAKE_PATH',
('Path2', GATE_POS))
gate_pos_commanded = True
plan_index, move_commanded = order_drones_movement(action_list, log_scd,
log_drn, plan_index,
move_commanded)
if move_commanded:
if len(episode_experience) > 0:
# save current state as next state to previous experience
episode_experience[-1][5] = log_scd.pos
episode_experience[-1][6] = log_drn.pos
episode_experience[-1][7] = log_ugv.pos
episode_experience[-1][8] = sniper.pos
# save current state and action
episode_experience.append([
log_scd.pos, log_drn.pos, log_ugv.pos, sniper.pos, plan_index,
Pos(),
Pos(),
Pos(),
Pos(), 0
])
order_drones_look_at(action_list, log_scd, log_drn)
return plan_index, stimulation_1_step, stimulation_2_step, gate_pos_commanded, move_commanded, start_ambush_step, attack2_commanded
def string_positions_list(str_list):
l = len(str_list)
assert l % 3 == 0
positions = []
for i in range(int(l / 3)):
positions.append(Pos(float(str_list[i * 3]), float(str_list[i * 3 + 1]), float(str_list[i * 3 + 2])))
return positions
def attack_enemy(action_list, entities_with_los_to_enemy, log_scd, log_ugv,
log_sensor):
attacked_enemies = []
if log_ugv in entities_with_los_to_enemy:
# ugv.attack(ENEMY_POS)
for enemy in log_ugv.los_enemies:
attacked_enemies.append(enemy)
add_action(action_list, log_ugv, 'ATTACK', (enemy.pos, ))
print("UGV attack:" + enemy.pos.__str__())
elif log_scd in entities_with_los_to_enemy:
# suicide.attack(ENEMY_POS)
for enemy in log_scd.los_enemies:
if enemy not in attacked_enemies:
attacked_enemies.append(enemy)
add_action(action_list, log_scd, 'ATTACK', (enemy.pos, ))
print("SUICIDE attack:" + enemy.pos.__str__())
else:
for enemy in log_sensor.los_enemies:
if enemy not in attacked_enemies:
# suicide.goto(ENEMY_POS)
add_action(
action_list, log_scd, 'MOVE_TO',
(Pos(enemy.gpoint.x, enemy.gpoint.y, log_scd.gpoint.z), ))
print("DRONE attack:" + enemy.pos.__str__())
def __init__(self, msg):
# def __init__(self, msg, state='zero'):
self.id = msg.id
self.diagstatus = msg.diagstatus
self.gpoint = Point()
self.imu = Imu()
self.health = KeyValue()
self.twist = Twist()
self._pos = Pos()
self._los_enemies = []
def __init__(self, msg):
self.cep = msg.cep
# self.gpoint = msg.gpose # self.gpoint = Point(x=-0.000204155, y=0.00035984, z=0.044715006)
self.gpoint = msg.gpose #Point(x=40.0, y=-23.0, z=0.044715006)
self.priority = msg.priority
self.tclass = msg.tclass
self.is_alive = msg.is_alive
# self.is_alive = msg.is_alive
self.id = msg.id
self._pos = Pos()
def get_env_and_entities(env, is_logical):
"""
Source for gym env and entities
Args:
env: PlannerEnv
is_logical: bool
Returns:
env: PlannerEnv
drn: PlannerEnv.Entity if _is_logical else SensorDrone
scd: PlannerEnv.Entity if _is_logical else SuicideDrone
ugv: PlannerEnv.Entity if _is_logical else Ugv
"""
global UGV_START_POS, SUICIDE_DRONE_START_POS, SENSOR_DRONE_START_POS
ugv_entity = env.get_entity('UGV')
scd_entity = env.get_entity('Suicide')
drn_entity = env.get_entity('SensorDrone')
while not bool(ugv_entity):
ugv_entity = env.get_entity('UGV')
ugv = lg_ugv('UGV', UGV_START_POS if is_logical else Pos(ugv_entity.gpoint.x, ugv_entity.gpoint.y,
ugv_entity.gpoint.z)) \
if is_logical else ugv_entity
while not bool(scd_entity):
scd_entity = env.get_entity('Suicide')
scd = lg_scd_drone('Suicide',
SUICIDE_DRONE_START_POS if is_logical else Pos(scd_entity.gpoint.x, scd_entity.gpoint.y,
scd_entity.gpoint.z)) \
if is_logical else scd_entity
while not bool(drn_entity):
drn_entity = env.get_entity('SensorDrone')
drn = lg_scn_drone('SensorDrone',
SENSOR_DRONE_START_POS if is_logical else Pos(drn_entity.gpoint.x, drn_entity.gpoint.y,
drn_entity.gpoint.z)) \
if is_logical else drn_entity
if is_logical:
log_entities = {drn.id: drn, scd.id: scd, ugv.id: ugv}
log_enemies = [sim_enemy(e) for e in env.enemies]
env = LogicSim(log_entities, log_enemies)
return env, drn, scd, ugv
def read_positions():
import csv
positions_dict = {}
irrelevant_keys = ['Ellipse1']
paths_key = ['Path1', 'Path2']
with open('PlannerPositions.csv', newline='') as csvfile:
reader = csv.reader(csvfile, delimiter=',', quotechar='|')
next(reader)
for row in reader:
key = row[0]
if key not in irrelevant_keys:
positions_dict[row[0]] = Pos(
float(row[5]), float(row[6]), float(row[7])) if key not in paths_key else string_positions_list(
[field for field in row[5:] if bool(field)])
return positions_dict
'gail': GAIL
}
JOBS = ['train', 'record', 'BC_agent', 'play']
POLICIES = ['MlpPolicy', 'CnnPolicy', 'CnnMlpPolicy']
BEST_MODELS_NUM = 0
EPS = 1e-6 # Avoid NaN (prevents division by zero or log of zero)
# CAP the standard deviation of the actor
LOG_STD_MAX = 2
LOG_STD_MIN = -20
lg_ugv.paths = {
'Path1': [Pos(29.9968816, 32.9994866, 1.75025599),
Pos(29.9969181, 32.9994912, 2.30123867),
Pos(29.9973316, 32.9996937, 1.02103409),
Pos(29.9977419, 32.9998162, 2.34626527),
Pos(29.9983693, 33.0001438, 1.14929717),
Pos(29.9987616, 33.0002219, 3.81971129),
Pos(29.9991068, 33.0002142, 0.213150453),
Pos(29.9992864, 33.0001952, -0.182928821),
Pos(29.9993341, 33.0001884, -0.180931998),
Pos(29.9993667, 33.0002117, -0.18193179)
],
'Path2': [Pos(29.9993825, 33.0002122, -0.390682418),
Pos(29.9995118, 33.0002082, -0.390672229),
Pos(29.9995114, 33.0002533, -0.390669329),
Pos(29.999509, 33.0002783, -0.00499354924)
]
def pos_to_point(pos: Pos) -> Point:
lon, lat, alt = pos.toLongLatAlt()
return Point(x=lat, y=lon, z=alt)
def point_to_pos(point: Point) -> Pos:
return Pos(point.x, point.y, point.z)
import copy
import logging
import sys
from logic_simulator.suicide_drone import SuicideDrone
from logic_simulator.sensor_drone import SensorDrone
from logic_simulator.drone import Drone
from logic_simulator.ugv import Ugv
from logic_simulator.pos import Pos
from logic_simulator.logic_sim import LogicSim
from logic_simulator.enemy import Enemy
LOGGER_LEVEL = logging.INFO
Ugv.paths = {
'Path1': [Pos(-47.0, -359.0, 1.00792499),
Pos(-49.0, -341.0, 1.04790355),
Pos(-29.0, -295.0, 0.40430533),
Pos(-17.0, -250.0, 1.06432373),
Pos(14.0, -180.0, 0.472875877),
Pos(22.0, -137.0, 1.80694756),
Pos(21.0, -98.0, 0.002950645),
Pos(19.0, -78.0, - 0.194334967),
Pos(17.0, -72.0, - 0.000997688),
Pos(19.0, -71.0, - 0.194334959)
],
'Path2': [Pos(19.0, -72.0, - 0.194336753),
Pos(26.0, -62.0, - 0.001001044),
Pos(26.0, -54.0, - 0.001001044),
Pos(27.0, -54.0, - 0.001000144)
]
def play(save_dir, env):
# action_list = {'MOVE_TO': [{}], 'LOOK_AT': [{}], 'ATTACK': [{}], 'TAKE_PATH': [{}]}
at_scanner1 = Pos(29.9994623, 33.0012379, 32.9879463)
at_scanner2 = Pos(29.9999499, 33.0010976, 29.3279355)
at_scanner3 = Pos(30.0001362, 33.0003552, 27.6133062)
at_house1 = Pos(29.9994717, 33.0004931, 3.47207769)
at_house2 = Pos(29.9995199, 33.0005282, 10.00616)
at_house3 = Pos(29.9995706, 33.0004819, 9.53707147)
at_suicide1 = Pos(29.9993852, 33.0008575, 20.5126056)
at_suicide2 = Pos(29.999815, 33.0008424, 21.0201285)
# at_suicide3 = Pos(x=-0.000118638, y=0.000438844, z=19.8076561)
at_point1 = Point(x=29.9993667, y=33.0002117, z=-0.18193179)
at_point2 = Point(x=29.9995096, y=33.0002783, z=-0.00499354924)
at_window1 = Point(x=29.9994918, y=33.0004458, z=10.015047)
lg_scanner1 = Pos(120.0, -59.0, 25.4169388)
lg_scanner2 = Pos(106.0, -5.0, 23.7457948)
lg_scanner3 = Pos(34.0, 16.0, 23.2363824)
lg_house1 = Pos(48.0, -58.0, 3.47494173)
lg_house2 = Pos(51.0, -52.0, 3.94403049)
lg_house3 = Pos(47.0, -47.0, 3.4749414)
lg_suicide1 = Pos(83.0, -67.0, 20.2996388)
lg_suicide2 = Pos(81.0, -20.0, 20.5166231)
lg_suicide3 = Pos(49.0, -13.0, 19.8076557)
lg_point1 = lg_ugv.paths['Path1'][-1]
lg_point2 = lg_ugv.paths['Path2'][-1]
lg_window1 = Pos(43.0, -56.0, 3.95291735)
timer_x_period = 10.0 # First timer UGV
timer_y_period = 10.0 # Second timer UGV
timer_zz_period = 10.0 # Timer for suicide sent to seen enemy
start_time_x = 0.0
start_time_y = 0.0
start_time_zz = 0.0
min_dist = 1.9
end_of_session = False
session_num = 1
root = configure_logger()
root.setLevel(logging.DEBUG)
while not end_of_session:
action_list = {
'MOVE_TO': [],
'LOOK_AT': [],
'ATTACK': [],
'TAKE_PATH': []
}
print("LALALALA - Starting session: session ", session_num)
curr_reward, curr_step, curr_num_of_dead, curr_num_of_lost_devices = \
run_logical_sim(env, is_logical=False)
# run_scenario(action_list, at_house1, at_house2, at_point1, at_point2, at_scanner1, at_scanner2, at_scanner3,
# at_suicide1, at_suicide2, at_suicide3, at_window1, env, min_dist, start_time_x, start_time_y,
# start_time_zz, timer_x_period, timer_y_period, timer_zz_period)
f = open("results.csv", "a")
curr_string = datetime.datetime.now().__str__() + "," + curr_reward.__str__() + "," + curr_step.__str__() + "," + curr_num_of_dead.__str__() + \
"," + curr_num_of_lost_devices.__str__() + "," + "\n"
f.write(curr_string)
f.close()
session_num += 1
if session_num > 10000:
end_of_session = True
# Print reward
print(curr_reward, curr_step, curr_num_of_dead,
curr_num_of_lost_devices)
'gail': GAIL
}
JOBS = ['train', 'record', 'BC_agent', 'play']
POLICIES = ['MlpPolicy', 'CnnPolicy', 'CnnMlpPolicy']
BEST_MODELS_NUM = 0
EPS = 1e-6 # Avoid NaN (prevents division by zero or log of zero)
# CAP the standard deviation of the actor
LOG_STD_MAX = 2
LOG_STD_MIN = -20
DISCOUNT_FACTOR = 0.95
lg_ugv.paths = {
'Path1': [
Pos(29.9968816, 32.9994866, 1.75025599),
Pos(29.9969181, 32.9994912, 2.30123867),
Pos(29.9973316, 32.9996937, 1.02103409),
Pos(29.9977419, 32.9998162, 2.34626527),
Pos(29.9983693, 33.0001438, 1.14929717),
Pos(29.9987616, 33.0002219, 3.81971129),
Pos(29.9991068, 33.0002142, 0.213150453),
Pos(29.9992864, 33.0001952, -0.182928821),
Pos(29.9993341, 33.0001884, -0.180931998),
Pos(29.9993667, 33.0002117, -0.18193179)
],
'Path2': [
Pos(29.9993825, 33.0002122, -0.390682418),
Pos(29.9995118, 33.0002082, -0.390672229),
Pos(29.9995114, 33.0002533, -0.390669329),
Pos(29.999509, 33.0002783, -0.00499354924)
def sim_point(point):
is_sim = True
return Pos(point.x, point.y, point.z) if is_sim else point
def sim_enemy(enemy):
return lg_enemy(enemy.id,
Pos(enemy.gpoint.x, enemy.gpoint.y, enemy.gpoint.z),
enemy.priority)