def update(self, start_pose, end_pose, Va, world_map, radius):
#generate tree
tree = MsgWaypoints()
#tree.type = 'straight_line'
tree.type = 'fillet'
# add the start pose to the tree
# check to see if start_pose connects directly to end_pose
# find path with minimum cost to end_node
waypoints_not_smooth = #find_minimum_path()
waypoints = #smooth_path()
return waypoints
bounding_box=(0, 0, 1000, 1000),
output_rate=SIM.ts_video)
# initialize elements of the architecture
wind = WindSimulation(SIM.ts_simulation)
mav = MavDynamics(SIM.ts_simulation)
autopilot = Autopilot(SIM.ts_simulation)
observer = Observer(SIM.ts_simulation, mav.true_state)
path_follower = PathFollower()
path_manager = PathManager()
# waypoint definition
from message_types.msg_waypoints import MsgWaypoints
waypoints = MsgWaypoints()
# waypoints.type = 'straight_line'
waypoints.type = 'fillet'
#waypoints.type = 'dubins'
Va = PLAN.Va0
waypoints.add(np.array([[0, 0, -100]]).T, Va, np.radians(0), np.inf, 0, 0)
waypoints.add(np.array([[1000, 0, -100]]).T, Va, np.radians(45), np.inf, 0, 0)
waypoints.add(np.array([[0, 1000, -100]]).T, Va, np.radians(45), np.inf, 0, 0)
waypoints.add(
np.array([[1000, 1000, -100]]).T, Va, np.radians(-135), np.inf, 0, 0)
# initialize the simulation time
sim_time = SIM.start_time
plot_timer = 0
# main simulation loop
print("Press Command-Q to exit...")
while sim_time < SIM.end_time: