def saop_trajectories_from_plan_msg(
msg: Plan) -> ty.Sequence[planning.Trajectory]:
# msg_tc.conf.uav_model
return [
planning.Trajectory(
planning.TrajectoryConfig(
msg_tc.conf.name,
planning.UAVModels.get(msg_tc.conf.uav_model),
planning.Waypoint(msg_tc.conf.start_wp.position.x,
msg_tc.conf.start_wp.position.y,
msg_tc.conf.start_wp.position.z,
msg_tc.conf.start_wp.orientation.psi),
planning.Waypoint(msg_tc.conf.end_wp.position.x,
msg_tc.conf.end_wp.position.y,
msg_tc.conf.end_wp.position.z,
msg_tc.conf.end_wp.orientation.psi),
rospy.Time.to_sec(msg_tc.conf.start_time),
msg_tc.conf.max_duration,
planning.WindVector(
msg_tc.conf.wind.speed *
np.cos(msg_tc.conf.wind.direction),
msg_tc.conf.wind.speed *
np.sin(msg_tc.conf.wind.direction))),
[
planning.TrajectoryManeuver(
planning.Waypoint(
m.waypoint.position.x, m.waypoint.position.y,
m.waypoint.position.z, m.waypoint.orientation.psi),
rospy.Time.to_sec(m.time), m.id) for m in msg_tc.maneuvers
]) for msg_tc in msg.trajectories
]
def set_initial_plan(self, name: str,
traj_conf: ty.Sequence[TrajectoryConf],
flight_window: ty.Tuple[float, float]):
"""Set up the planner for an initial plan"""
tc = [
planning.Trajectory(
planning.TrajectoryConfig(c.name, self.uav_models[c.uav_model],
planning.Waypoint(*c.start_wp),
planning.Waypoint(*c.end_wp),
c.start_time, c.max_duration,
planning.WindVector(*c.wind)))
for c in traj_conf
]
f_data = planning.make_fire_data(self._wildfire_map, self._elevation)
tw = planning.TimeWindow(*flight_window)
utility = planning.make_flat_utility_map(self._wildfire_map,
flight_window=tw,
output_layer="utility")
the_plan = planning.Plan(name, tc, f_data, tw,
utility.as_cpp_raster("utility"))
self._current_planner = planning.Planner(the_plan, {})
def set_plan(self, name: str, trajs: ty.Sequence[TrajType],
flight_window: ty.Tuple[float, float]):
"""Set up the planner for improving an existing plan"""
tr = [
planning.Trajectory(
planning.TrajectoryConfig(c[0].name,
self.uav_models[c[0].uav_model],
planning.Waypoint(*c[0].start_wp),
planning.Waypoint(*c[0].end_wp),
c[0].start_time, c[0].max_duration,
planning.WindVector(*c[0].wind)),
[
planning.TrajectoryManeuver(planning.Waypoint(*m[0]), m[1],
m[2]) for m in c[1]
]) for c in trajs
]
f_data = planning.make_fire_data(self._wildfire_map, self._elevation)
tw = planning.TimeWindow(*flight_window)
utility = planning.make_utility_map(self._wildfire_map,
flight_window=tw,
output_layer="utility")
the_plan = planning.Plan(name, tr, f_data, tw, utility)
self._current_planner = planning.Planner(the_plan, {})
tw = planning.TimeWindow(0, np.inf)
utility = planning.make_utility_map(fire_prop.ignitions(), flight_window=tw,
output_layer="utility")
trajectory_config = [planning.Trajectory(planning.TrajectoryConfig("Trajectory",
planning.UAVModels.x8("06"),
planning.Waypoint(
2801900.0 - 1500,
2298900.0 - 1500, 300.0,
0.0),
planning.Waypoint(
2801900.0 - 1500,
2298900.0 - 1500, 300.0,
0.0),
now,
2700.0,
planning.WindVector(3.0,
0.0)))]
the_plan = planning.Plan("observation_plan", trajectory_config, f_data, tw,
utility.as_cpp_raster("utility"))
initial_u_map = the_plan.utility_map()
planner = planning.Planner(the_plan, planning.VNSConfDB.demo_db()["demo"])
sr_1 = planner.compute_plan(10.0)
print("Flight mission length: {} minutes".format((sr_1.final_plan().trajectories()[0].start_times[-1] - sr_1.final_plan().trajectories()[0].start_times[0])/60.0))
## Display results
final_u_map = the_plan.utility_map()
gdd = display.GeoDataDisplay(
*display.get_pyplot_figure_and_axis(),
seganosa_fire_env.raster.combine(fire_prop.ignitions()),
frame=(0., 0.))