import parameters.planner_parameters as PLAN import parameters.aerosonde_parameters as P from chap3.data_viewer import data_viewer from chap4.wind_simulation import wind_simulation from chap6.autopilot import autopilot from chap7.mav_dynamics import mav_dynamics from chap8.observer_ekf import observer from chap10.path_follower import path_follower from chap11.path_manager import path_manager from chap12.world_viewer import world_viewer from project.voronoi import Voronoi_Planner from message_types.msg_map import msg_map # initialize elements of the architecture wind = wind_simulation(SIM.ts_simulation) mav = mav_dynamics(SIM.ts_simulation, [P.pn0_corner, P.pe0_corner, P.pd0]) ctrl = autopilot(SIM.ts_simulation) obsv = observer(SIM.ts_simulation, [P.pn0_corner, P.pe0_corner, P.pd0]) path_follow = path_follower() path_manage = path_manager() map = msg_map(PLAN) voronoi = Voronoi_Planner(map) # -------path planner - ---- waypoints = voronoi.plan() waypoints.ned = waypoints.ned[:, :waypoints.num_waypoints] # initialize the visualization VIDEO = False # True==write video, False==don't write video world_view = world_viewer(map, voronoi) # initialize the viewer
def main():
if DATA:
screen_pos = [2000, 0] # x, y position on screen
data_view = data_viewer(*screen_pos) # initialize view of data plots
# Holodeck Setup
env = holodeck.make("Ocean", show_viewport=True)
env.reset()
# wave intensity: 1-13, wave size: 1-8, wave dir: 0-360 deg
env.set_ocean_state(6, 3, 90)
env.set_day_time(5)
env.set_weather('Cloudy')
env.set_aruco_code(False)
alt = -PLAN.MAV.pd0*100
if DEBUG_HOLO:
env.set_control_scheme("uav0", ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT)
uav_cmd = np.array([0, -0.2, 0, alt])
uav_cmd = np.array([0,0,0,0])
boat_cmd = 0
env.act("uav0", uav_cmd)
env.act("boat0", boat_cmd)
# Just for getting UAV off the platform
state = env.set_state("uav0", [-1500, 0, 6000], [0,0,0], [0,0,0], [0,0,0])["uav0"]
state = env.set_state("uav0", [-1500, 0, 2000], [0,0,0], [0,0,0], [0,0,0])["uav0"]
env.teleport("boat0", location=[3500, 0, 0], rotation=[0, 0, 0])
# Initialization
pos = np.array([OFFSET_N, OFFSET_E, alt])
att = np.array([0, 0, 0])
vel = np.array([0, 0, 0]) * 100
angvel = np.array([0, 0, 0])
state = env.set_state("uav0", pos, att, vel, angvel)["uav0"]
# Camera
cam_alt = 300
env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
if DEBUG_HOLO:
pos = state["LocationSensor"]
# env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
for i in range(1):
state = env.tick()["uav0"]
pos = state["LocationSensor"]
# env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
if SHOW_PIXELS:
pixels = state["RGBCamera"]
cv2.imshow('Camera', pixels)
cv2.waitKey(0)
print("READY FOR SIM")
# sys.exit(0)
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
ctrl = autopilot(SIM.ts_simulation)
obsv = observer(SIM.ts_simulation)
path_follow = path_follower()
path_manage = path_manager()
# waypoint definition
waypoints = msg_waypoints()
waypoints.type = 'straight_line'
waypoints.type = 'fillet'
waypoints.type = 'dubins'
waypoints.num_waypoints = 4
Va = PLAN.Va0
d = 1000.0
h = -PLAN.MAV.pd0
waypoints.ned[:waypoints.num_waypoints] = np.array([[0, 0, -h],
[d, 0, -h],
[0, d, -h],
[d, d, -h]])
waypoints.airspeed[:waypoints.num_waypoints] = np.array([Va, Va, Va, Va])
waypoints.course[:waypoints.num_waypoints] = np.array([np.radians(0),
np.radians(45),
np.radians(45),
np.radians(-135)])
# initialize the simulation time
sim_time = SIM.start_time
delta = np.zeros(4)
mav.update(delta) # propagate the MAV dynamics
mav.update_sensors()
# main simulation loop
# print("Waiting for keypress")
# input()
print("Press Q to exit...")
while sim_time < SIM.end_time:
#-------observer-------------
measurements = mav.update_sensors() # get sensor measurements
# estimate states from measurements
estimated_state = obsv.update(measurements)
#-------path manager-------------
path = path_manage.update(waypoints, PLAN.R_min, estimated_state)
#-------path follower-------------
autopilot_commands = path_follow.update(path, estimated_state)
# autopilot_commands = path_follow.update(path, mav.true_state)
#-------controller-------------
delta, commanded_state = ctrl.update(autopilot_commands, estimated_state)
#-------physical system------j-------
current_wind = wind.update() # get the new wind vector
mav.update(delta, current_wind) # propagate the MAV dynamics
#-------update holodeck-------------
update_holodeck(env, mav.true_state)
draw_holodeck(env, waypoints)
#-------update viewer-------------
if DATA:
data_view.update(mav.true_state, # true states
estimated_state, # estimated states
commanded_state, # commanded states
SIM.ts_simulation)
#-------increment time-------------
sim_time += SIM.ts_simulation
