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
def smooth_path(waypoints, world_map):
# smooth the waypoint path
smooth = [0] # add the first waypoint
# construct smooth waypoint path
smooth_waypoints = MsgWaypoints()
return smooth_waypoints
def find_minimum_path(tree, end_pose):
# find the lowest cost path to the end node
# find nodes that connect to end_node
connecting_nodes = []
# find minimum cost last node
idx =
# construct lowest cost path order
path =
# construct waypoint path
waypoints = MsgWaypoints()
return waypoints
from chap2.video_writer import VideoWriter
video = VideoWriter(video_name="chap11_video.avi",
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
def __init__(self):
# waypoints definition
self.waypoints = MsgWaypoints()
self.rrt_straight_line = RRTStraightLine()
self.rrt_dubins = RRTDubins()
class PathPlanner:
def __init__(self):
# waypoints definition
self.waypoints = MsgWaypoints()
self.rrt_straight_line = RRTStraightLine()
self.rrt_dubins = RRTDubins()
def update(self, world_map, state, radius):
print('planning...')
# this flag is set for one time step to signal a redraw in the viewer
# planner_flag = 'simple_straight' # return simple waypoint path
# planner_flag = 'simple_dubins' # return simple dubins waypoint path
planner_flag = 'rrt_straight' # plan path through city using straight-line RRT
#planner_flag = 'rrt_dubins' # plan path through city using dubins RRT
if planner_flag == 'simple_straight':
Va = 25
self.waypoints.type = 'fillet'
self.waypoints.add(
np.array([[0, 0, -100]]).T, Va, np.inf, np.inf, 0, 0)
self.waypoints.add(
np.array([[1000, 0, -100]]).T, Va, np.inf, np.inf, 0, 0)
self.waypoints.add(
np.array([[0, 1000, -100]]).T, Va, np.inf, np.inf, 0, 0)
self.waypoints.add(
np.array([[1000, 1000, -100]]).T, Va, np.inf, np.inf, 0, 0)
elif planner_flag == 'simple_dubins':
Va = 25
self.waypoints.type = 'dubins'
self.waypoints.add(
np.array([[0, 0, -100]]).T, Va, np.radians(0), np.inf, 0, 0)
self.waypoints.add(
np.array([[1000, 0, -100]]).T, Va, np.radians(45), np.inf, 0,
0)
self.waypoints.add(
np.array([[0, 1000, -100]]).T, Va, np.radians(45), np.inf, 0,
0)
self.waypoints.add(
np.array([[1000, 1000, -100]]).T, Va, np.radians(-135), np.inf,
0, 0)
elif planner_flag == 'rrt_straight':
desired_airspeed = 25
desired_altitude = 100
# start pose is current pose
start_pose = np.array([[state.north], [state.east],
[-desired_altitude]])
# desired end pose
if np.linalg.norm(start_pose[0:2]) < world_map.city_width / 2:
end_pose = np.array([[world_map.city_width],
[world_map.city_width],
[-desired_altitude]])
else: # or to the bottom-left corner of world_map
end_pose = np.array([[0], [0], [-desired_altitude]])
self.waypoints = self.rrt_straight_line.update(
start_pose, end_pose, desired_airspeed, world_map, radius)
elif planner_flag == 'rrt_dubins':
desired_airspeed = 25
desired_altitude = 100
# start pose is current pose
start_pose = np.array([[state.north], [state.east],
[-desired_altitude], [state.chi]])
# desired end pose
# either plan to the top-right corner of world_map
if np.linalg.norm(start_pose[0:2]) < world_map.city_width / 2:
end_pose = np.array([[world_map.city_width],
[world_map.city_width],
[-desired_altitude], [state.chi]])
else: # or to the bottom-left corner of world_map
end_pose = np.array([[0], [0], [-desired_altitude],
[state.chi]])
self.waypoints = self.rrt_dubins.update(start_pose, end_pose,
desired_airspeed,
world_map, radius)
else:
print("Error in Path Planner: Undefined planner type.")
self.waypoints.plot_updated = False
print('...done planning.')
return self.waypoints