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