def publish_plan(self, plan_prototype: Plan, saop_plan):
"""Publish current plan."""
for (traj_msg, traj_plan) in zip(plan_prototype.trajectories, saop_plan.trajectories()):
names = traj_plan.maneuver_names
starts = traj_plan.start_times
segs = traj_plan.segments
maneuver_msg_list = [Maneuver(n, rospy.Time.from_sec(t),
PoseEuler(position=Point(s.start.x, s.start.y, s.start.z),
orientation=Euler(.0, .0, s.start.dir)))
for n, t, s in zip(names, starts, segs)]
traj_msg.maneuvers = maneuver_msg_list
g = GeoDataDisplay.pyplot_figure(self.op._elevation)
g.add_extension(TrajectoryDisplayExtension, (None,), {})
plot_plan_trajectories(saop_plan, g, trajectories=slice(None),
layers=["bases", "arrows", "trajectory_solid"])
g.figure.savefig("plan.svg")
self.pub_plan.publish(plan_prototype)
def plot_sr_with_background(sr: 'planning.up.SearchResult', geodatadisplay, time_range,
output_options_plot,
plan: 'Union[str, int]' = 'final'):
"""Plot a plan trajectories with background geographic information in a geodata display."""
# Draw background layers
for layer in output_options_plot['background']:
if layer == 'elevation_shade':
geodatadisplay.draw_elevation_shade(
with_colorbar=output_options_plot.get('colorbar', True), layer='elevation')
if layer == 'elevation_planning_shade':
geodatadisplay.draw_elevation_shade(
with_colorbar=output_options_plot.get('colorbar', True), layer='elevation_planning')
elif layer == 'ignition_shade':
geodatadisplay.draw_ignition_shade(
with_colorbar=output_options_plot.get('colorbar', True))
# elif layer == 'observedcells':
# geodatadisplay.TrajectoryDisplayExtension.draw_observation_map(
# planner.expected_observed_map(layer_name="expected_observed"),
# layer='expected_observed', color='green', alpha=0.9)
# geodatadisplay.TrajectoryDisplayExtension.draw_observation_map(
# planner.expected_ignited_map(layer_name="expected_ignited"),
# layer='expected_ignited', color='red', alpha=0.9)
elif layer == 'ignition_contour':
try:
geodatadisplay.draw_ignition_contour(with_labels=True)
except ValueError as e:
logger.exception("ValueError while drawing ignition contour")
elif layer == 'wind_quiver':
geodatadisplay.draw_wind_quiver()
elif layer == 'utilitymap':
geodatadisplay.TrajectoryDisplayExtension.draw_utility_shade(
geodata=GeoData.from_cpp_raster(sr.final_plan().utility_map(), 'utility'),
layer='utility', vmin=0., vmax=1.)
plot_plan_trajectories(sr.plan(plan), geodatadisplay,
layers=output_options_plot["foreground"], time_range=time_range)
sr_1 = pl.search_result
fmapper = FireMapper(env, fire1) # fire3 is the ground thruth
####################################
# 1st figure: show only the first plan
gdd = fire_rs.geodata.display.GeoDataDisplay.pyplot_figure(env.raster.combine(fire1),
frame=(0, 0))
gdd.add_extension(TrajectoryDisplayExtension, (None,), {})
# Draw expected fire contour
t_range = (sr_1.final_plan().trajectories()[0].start_time(0) - 120,
sr_1.final_plan().trajectories()[0].end_time(len(
sr_1.final_plan().trajectories()[0]) - 1) + 120)
gdd.draw_ignition_contour(geodata=fire1, time_range=t_range, cmap=matplotlib.cm.Reds)
# Draw observed fire
# executed_path_1 = sr_1.final_plan().trajectories()[0].sampled_with_time(step_size=10)
# fmapper.observe(executed_path_1, pl.flights[0].uav)
# gdd.draw_ignition_shade(geodata=fmapper.firemap, cmap=matplotlib.cm.viridis,
# vmin=t_range[0], vmax=t_range[1])
# Draw trajectory
plot_plan_trajectories(sr_1.final_plan(), gdd, trajectories=0, draw_path=False,
draw_flighttime_path=True, colors=['blue'], labels=["executed"],
linestyles=['-'])
gdd.legend()
gdd.figure.show()
print(pl.search_result)
# First plan
pl = Planner(env, fire.ignitions(), [fgconf], conf)
pl.compute_plan()
sr_1 = pl.search_result
ei_1 = pl.expected_ignited_map(layer_name="expected_ignited")
eo_1 = pl.expected_observed_map(layer_name="expected_observed")
gdd = fire_rs.geodata.display.GeoDataDisplay(
*fire_rs.geodata.display.get_pyplot_figure_and_axis(),
env.raster.combine(fire.ignitions()).combine(ei_1).combine(eo_1))
gdd.add_extension(TrajectoryDisplayExtension, (None, ), {})
gdd.draw_ignition_contour()
gdd.draw_observation_map(layer='expected_observed',
color='darkgreen',
alpha=0.5)
gdd.draw_observation_map(layer='expected_ignited',
color='darkred',
alpha=0.5)
plot_plan_trajectories(sr_1.final_plan(),
gdd,
colors=["maroon"],
show=True)
upload_plan("127.0.0.1", "6002", sr_1.final_plan(),
"Fire plan long obs sampled(-1)", 50., -1)
start_plan("127.0.0.1", "6002", "Fire plan long obs sampled(-1)")
set_wind("127.0.0.1", "6002", *wind)
print("Expected duration: " + str(sr_1.final_plan().duration()))
print(" - - END - - ")
def threefire_twouav_figure(
wind_speed=5.,
wind_dir=np.pi / 2) -> "fire_rs.geodata.display.GeoDataDisplay":
"""Plot a generic case with wind, multiple UAV from multiple bases, and multiple fire."""
# Geographic environment (elevation, landcover, wind...)
wind = (wind_speed, wind_dir)
area = ((480000.0, 485000.0), (6210000.0, 6215000.0))
env = PlanningEnvironment(area,
wind_speed=wind[0],
wind_dir=wind[1],
planning_elevation_mode='flat',
flat_altitude=0)
# Fire applied to the previous environment
ignition_points = [
TimedPoint(area[0][0] + 1000.0, area[1][0] + 1000.0, 0.),
TimedPoint(area[0][0] + 4000.0, area[1][0] + 2000.0, 0.),
TimedPoint(area[0][0] + 2000.0, area[1][0] + 3500.0, 0.)
]
logging.info("Start of propagation")
fire = propagation.propagate_from_points(env, ignition_points, 120 * 60)
logging.info("End of propagation")
# Configure some flight
start_t = 90 * 60
uav = UAVConf.x8()
uav.max_flight_time /= 3
f_confs = [
FlightConf(uav, start_t,
Waypoint(area[0][0] + 100., area[1][0] + 100., 0., 0.),
None, wind),
FlightConf(uav, start_t,
Waypoint(area[0][0] + 100., area[1][0] + 100., 0., 0.),
None, wind),
]
conf = SAOP_conf(start_t, start_t + uav.max_flight_time)
fire1 = fire.ignitions()
pl = Planner(env, fire1, f_confs, conf)
pl.compute_plan()
sr_1 = pl.search_result
fmapper = FireMapper(env, fire1)
gdd = fire_rs.geodata.display.GeoDataDisplay.pyplot_figure(
env.raster.combine(fire1), frame=(0, 0))
gdd.add_extension(TrajectoryDisplayExtension, (None, ), {})
# Draw expected fire contour
t_range = (sr_1.final_plan().trajectories()[0].start_time(0),
sr_1.final_plan().trajectories()[0].end_time(
len(sr_1.final_plan().trajectories()[0]) - 1))
t_range_fire = (0, np.inf)
gdd.draw_ignition_contour(geodata=fire1,
time_range=t_range_fire,
cmap=matplotlib.cm.Reds,
alpha=1)
# Draw observed fire
for i in range(len(f_confs)):
executed_path = sr_1.final_plan().trajectories()[i].sampled_with_time(
step_size=10)
fmapper.observe(executed_path, pl.flights[i].uav)
gdd.draw_ignition_shade(geodata=fmapper.firemap,
cmap=matplotlib.cm.summer,
vmin=t_range[0],
vmax=t_range[1],
with_colorbar=False)
# Draw trajectory
colors = ['blue', 'darkgreen', 'magenta']
labels = ["UAV " + str(i) for i in range(len(f_confs))]
for i in range(len(f_confs)):
plot_plan_trajectories(sr_1.final_plan(),
gdd,
trajectories=i,
draw_path=True,
draw_flighttime_path=False,
colors=[colors[i]],
labels=[labels[i]],
linestyles=['-'])
gdd.legend()
return gdd
def detail_case_figure(
wind_speed=5.,
wind_dir=0.) -> "fire_rs.geodata.display.GeoDataDisplay":
"""Plot a case with an small fire
we can make a zoom and observe a dubinswind path and observed cells."""
# Geographic environment (elevation, landcover, wind...)
wind = (wind_speed, wind_dir)
wind_xy = (wind[0] * np.cos(wind[1]), wind[0] * np.sin(wind[1]))
area = ((480000.0, 485000.0), (6210000.0, 6215000.0))
env = PlanningEnvironment(area,
wind_speed=10,
wind_dir=wind[1],
planning_elevation_mode='flat',
flat_altitude=0)
# Fire applied to the previous environment
ignition_points = [
TimedPoint(area[0][0] + 2000.0, area[1][0] + 2000.0, 0),
]
logging.info("Start of propagation")
fire = propagation.propagate_from_points(env, ignition_points, 180 * 60)
logging.info("End of propagation")
# Configure some flight
base_wp = Waypoint(area[0][0] + 100., area[1][0] + 100., 0., 0.)
start_t = 120 * 60
uav = UAVConf.x8()
f_confs = [
FlightConf(uav, start_t,
Waypoint(area[0][0] + 100., area[1][0] + 100., 0., 0.),
None, wind_xy),
]
conf = SAOP_conf(start_t, start_t + uav.max_flight_time)
fire1 = fire.ignitions()
pl = Planner(env, fire1, f_confs, conf)
pl.compute_plan()
sr_1 = pl.search_result
fmapper = FireMapper(env, fire1)
gdd = fire_rs.geodata.display.GeoDataDisplay.pyplot_figure(
env.raster.combine(fire1), frame=(0, 0))
gdd.add_extension(TrajectoryDisplayExtension, (None, ), {})
gdd.axes.grid(True)
# Draw expected fire contour
t_range = (sr_1.final_plan().trajectories()[0].start_time(0) - 120,
sr_1.final_plan().trajectories()[0].end_time(
len(sr_1.final_plan().trajectories()[0]) - 1) + 120)
t_range_fire = (0, np.inf)
gdd.draw_ignition_contour(geodata=fire1,
time_range=t_range_fire,
cmap=matplotlib.cm.Reds)
# Draw observed fire
executed_path_1 = sr_1.final_plan().trajectories()[0].sampled_with_time(
step_size=10)
fmapper.observe(executed_path_1, pl.flights[0].uav)
gdd.TrajectoryDisplayExtension.draw_observation_map(
obs_map=fmapper.observed, layer='ignition', color='gray')
gdd.TrajectoryDisplayExtension.draw_observation_map(
obs_map=fmapper.firemap, layer='ignition', color='green')
# Draw trajectory
colors = ['blue', 'green', 'magenta']
labels = ["UAV " + str(i) for i in range(len(f_confs))]
for i in range(len(f_confs)):
plot_plan_trajectories(sr_1.final_plan(),
gdd,
trajectories=i,
draw_path=True,
draw_segments=False,
draw_flighttime_path=False,
colors=[colors[i]],
labels=None)
gdd.legend()
return gdd
cmap=matplotlib.cm.Purples)
# Draw observed fire
executed_path_1 = sr_1.final_plan().trajectories()[0].sampled_with_time(
(0, from_t_2), step_size=10)
fmapper.observe(executed_path_1, pl.flights[0].uav)
gdd.draw_ignition_shade(geodata=fmapper.firemap,
cmap=matplotlib.cm.viridis,
vmin=t_range[0],
vmax=t_range[1])
# Draw trajectory
plot_plan_trajectories(sr_1.final_plan(),
gdd,
trajectories=0,
draw_segments=False,
colors=['blue'],
labels=["1st plan"],
linestyles=[':'])
plot_plan_trajectories(sr_1.final_plan(),
gdd,
time_range=(0, from_t_2 - 60),
trajectories=0,
colors=['blue'],
labels=["executed"],
linestyles=['-'])
gdd.legend()
gdd.figure.show()
####################################
# 2nd figure: First and second plan
####################################
# 1st PLAN
fire1 = fire.ignitions()
# pl = Planner(env, fire1, [fgconf_1, fgconf_2], conf)
pl = Planner(env, fire1, [fgconf_1], conf)
pl.compute_plan()
sr_1 = pl.search_result
gdd = fire_rs.geodata.display.GeoDataDisplay.pyplot_figure(
env.raster.combine(fire1), frame=(0, 0))
gdd.add_extension(TrajectoryDisplayExtension, (None, ), {})
gdd.draw_elevation_shade()
gdd.draw_ignition_contour()
plot_plan_trajectories(pl.search_result.final_plan(),
gdd,
trajectories=slice(None),
colors=["blue", "darkgreen"],
layers=["bases", "arrows", "trajectory_solid"])
gdd.figure.show()
import os
for i in range(len(sr_1.intermediate_plans)):
int_gdd = fire_rs.geodata.display.GeoDataDisplay.pyplot_figure(
env.raster.combine(fire1), frame=(0, 0))
int_gdd.add_extension(TrajectoryDisplayExtension, (None, ), {})
# int_gdd.draw_elevation_shade()
int_gdd.draw_ignition_contour()
plot_plan_trajectories(sr_1.intermediate_plans[i],
int_gdd,
trajectories=slice(None),
# Obtain Obs fire using a firemapper over a fake ground truth.
# In normal conditions, Obs fire should be provided by neptus
elevation_draster = pl_env.raster.as_cpp_raster('elevation')
ignition_draster = ignitions.as_cpp_raster()
gfmapper = fmapping.GhostFireMapper(
op.FireData(ignition_draster, elevation_draster))
# Suposing we follow exactly the planned path
# executed_path is a tuple with a list of waypoints and a list of the corresponding time
executed_path = search_result.final_plan().trajectories(
)[0].sampled_with_time(step_size=10)
executed_path_wp = executed_path[0]
executed_path_t = executed_path[1]
# type: 'op.DRaster'
obs_fire_raster = gfmapper.observed_firemap(executed_path_wp,
executed_path_t,
flight_confs_cpp[0].uav)
obs_fire = GeoData.from_cpp_raster(obs_fire_raster, 'ignition')
gdd = GeoDataDisplay.pyplot_figure(obs_fire)
gdd.add_extension(TrajectoryDisplayExtension, (None, ))
gdd.draw_ignition_contour(geodata=ignitions, cmap=mcm.Greens)
gdd.draw_ignition_shade()
plot_plan_trajectories(search_result.final_plan(), gdd)
print("fin")
# TODO: Reconstruct fire perimeter from Obs fire
# search_result = observation_planner.replan_after_time(later)
