def __init__(self, known_uavs: ty.Sequence[str]):
rospy.init_node("fake_mapper")
rospy.loginfo("Starting {}".format(self.__class__.__name__))
self._known_uavs = known_uavs
# type: ty.Mapping[str, rospy.Publisher]
self.sub_uav_state_dict = {uav: rospy.Subscriber(
"/".join(("uavs", serialization.ros_name_for(uav), 'state')), VehicleState,
callback=self._on_vehicle_state, callback_args=uav, queue_size=10) for uav in
self._known_uavs}
# type: rospy.Subscriber
self.sub_elevation_map = rospy.Subscriber("elevation", ElevationMap, self._on_elevation_map,
queue_size=10)
# type: rospy.Subscriber
self.sub_real_wildfire_map = rospy.Subscriber("real_wildfire", WildfireMap,
self._on_real_wildfire_map,
queue_size=10)
# type: ty.Mapping[str, rospy.Publisher]
self.pub_dict_firemap = {
uav: rospy.Publisher(
"/".join(("uavs", serialization.ros_name_for(uav), 'wildfire_observed')),
WildfireMap, queue_size=10) for uav in self._known_uavs}
self.location_history = {} # type: ty.MutableMapping[str, ty.MutableSequence[ty.Any]]
self.elevation_map = None
self.real_wildfire_map = None
self.firemapper = None # type: ty.Optional[fire_rs.simulation.fakefiremapper.FakeFireMapper]
self.lock = threading.Lock()
def go_home(self, uav: str):
plan_name = "".join(("home", serialization.ros_name_for(uav)))
uav_model = UAVModels.get(uav)
home_cmd = HomeCmd(header=Header(stamp=rospy.Time.now()),
uav=serialization.ros_name_for(uav),
plan_name=plan_name,
loiter=Loiter(
id=plan_name,
center=PoseEuler(position=Point(
*self.hmi_model.uav_bases.data[uav], 200.),
orientation=Euler(0., 0., 0.)),
radius=uav_model.min_turn_radius,
direction=Loiter.DIRECTION_VDEP,
duration=rospy.Duration()))
self.pub_home_cmd.publish(home_cmd)
def __init__(self, area, world_paths=None):
rospy.init_node('situation_assessment')
rospy.loginfo("Starting {}".format(self.__class__.__name__))
self.sa = SituationAssessment(area, logging.getLogger(__name__), world_paths=world_paths)
# Lock from new observations while doing predictions
self.sa_lock = threading.Lock()
self.last_wildfire_update = datetime.datetime.min
self.last_elevation_timestamp = datetime.datetime.min
self.horizon = rospy.Duration(secs=3 * 60 * 60)
self.pub_wildfire_pred = rospy.Publisher('wildfire_prediction',
PredictedWildfireMap,
queue_size=10, latch=True)
self.pub_wildfire_current = rospy.Publisher('wildfire', WildfireMap,
queue_size=10, latch=True)
# self.pub_wildfire_real = rospy.Publisher('wildfire_real', WildfireMap,
# queue_size=10, latch=True)
self.pub_elevation = rospy.Publisher('elevation', ElevationMap,
queue_size=10, latch=True)
# self.pub_surface_wind = rospy.Publisher('surface_wind', SurfaceWindMap,
# queue_size=10, latch=True)
self.uavs = ('x8-02', 'x8-06')
self.sub_firemap_obs_dict = {
uav: rospy.Subscriber(
"/".join(("uavs", serialization.ros_name_for(uav), 'wildfire_observed')),
WildfireMap, callback=self._on_wildfire_map_observed, callback_args=uav,
queue_size=10) for uav in self.uavs}
self.sub_firemap_obs_alarm = rospy.Subscriber("wildfire_observed", WildfireMap,
callback=self._on_wildfire_map_observed,
callback_args="Alarm", queue_size=10)
self.sub_mean_wind = rospy.Subscriber("mean_wind", MeanWindStamped, self.on_mean_wind)
self.sub_propagate = rospy.Subscriber("propagate", PropagateCmd, self.on_propagate_cmd)
self.sub_wildfire_point = rospy.Subscriber("wildfire_point", Timed2DPointStamped,
self.on_wildfire_obs_point)
self.assessment_th = None # self.assessment_th = threading.Thread(None, self.propagate, daemon=True)
def __init__(self, model: HMIModel):
rospy.init_node("hmi")
self.elevation_map_cb = lambda x: None
self.hmi_model = model
self.hmi_model.set_model_provider(self)
self.uavs = ('x8-02', 'x8-06')
self.sub_rosout = rospy.Subscriber("rosout",
Log,
self._on_log,
queue_size=10)
self.sub_elevation_map = rospy.Subscriber("elevation",
ElevationMap,
self._on_elevation_map,
queue_size=10)
self.sub_wildfire_map = rospy.Subscriber(
"wildfire_prediction",
PredictedWildfireMap,
self._on_wildfire_map_predicted,
queue_size=10)
self.sub_wildfire_map = rospy.Subscriber("wildfire",
WildfireMap,
self._on_wildfire_map,
queue_size=10)
self.sub_uav_state_dict = {
uav: rospy.Subscriber("/".join(
("uavs", serialization.ros_name_for(uav), 'state')),
VehicleState,
callback=self._on_vehicle_state,
callback_args=uav,
queue_size=10)
for uav in self.uavs
}
self.sub_firemap_obs_dict = {
uav: rospy.Subscriber("/".join(
("uavs", serialization.ros_name_for(uav),
'wildfire_observed')),
WildfireMap,
callback=self._on_wildfire_map_observed,
callback_args=uav,
queue_size=10)
for uav in self.uavs
}
self.sub_windspeed_dict = {
uav: rospy.Subscriber("/".join(
("uavs", serialization.ros_name_for(uav), 'windspeed')),
MeanWindStamped,
callback=self._on_windspeed,
callback_args=uav,
queue_size=10)
for uav in self.uavs
}
self.sub_plan = rospy.Subscriber("plan",
Plan,
callback=self._on_plan,
queue_size=10)
self.pub_propagate = rospy.Publisher("propagate",
PropagateCmd,
queue_size=10)
self.pub_initial_plan = rospy.Publisher("initial_plan",
PlanCmd,
queue_size=10)
self.pub_stop = rospy.Publisher("stop", StopCmd, queue_size=10)
self.pub_home_cmd = rospy.Publisher("go_home", HomeCmd, queue_size=10)
def __init__(self, coordinate_system=None):
rospy.init_node('ccu_bridge')
rospy.loginfo("Starting {}".format(self.__class__.__name__))
if coordinate_system is not None:
if coordinate_system == geo_data.EPSG_ETRS89_LAEA:
# self.ccu.set_coordinate_system(geo_data.EPSG_ETRS89_LAEA, geo_data.EPSG_ETRS89)
pass
elif coordinate_system == geo_data.EPSG_RGF93_LAMBERT93:
# self.ccu.set_coordinate_system(geo_data.EPSG_RGF93_LAMBERT93, geo_data.EPSG_RGF93)
pass
elif coordinate_system == geo_data.EPSG_WGS84_UTM29N:
# self.ccu.set_coordinate_system(geo_data.EPSG_WGS84_UTM29N, geo_data.EPSG_WGS84)
pass
else:
rospy.logfatal("Unknown coordinate system code '%s'",
str(coordinate_system))
raise rospy.ROSInterruptException(
"Unknown coordinate system code '{}'".format(
str(coordinate_system)))
else:
rospy.logwarn(
"No coordinate system has been set. Using default Lambert93/RGF93"
)
self.ccu = NeptusBridge(logging.getLogger(__name__), coordinate_system)
self.ccu.start()
self.ccu.set_uav_state_callback(self.on_uav_state_from_neptus)
self.ccu.set_trajectory_state_callback(
self.on_trajectory_state_from_neptus)
self.ccu.set_firemap_callback(self.on_firemap_from_neptus)
self._known_uavs = set()
self._known_uavs.add('x8-06')
self._known_uavs.add('x8-02')
self.dict_state_lock = threading.Lock()
self.dict_state_msg = {uav: None for uav in self._known_uavs}
# type: ty.Mapping[str, rospy.Publisher]
self.pub_dict_state = {
uav: rospy.Publisher("/".join(
("uavs", serialization.ros_name_for(uav), 'state')),
VehicleState,
queue_size=10)
for uav in self._known_uavs
}
self.dict_traj_lock = threading.Lock()
self.dict_traj_msg = {uav: None for uav in self._known_uavs}
# type: ty.Mapping[str, rospy.Publisher]
self.pub_dict_traj = {
uav: rospy.Publisher("/".join(
("uavs", serialization.ros_name_for(uav), 'trajectory')),
TrajectoryState,
queue_size=10)
for uav in self._known_uavs
}
self.dict_firemap_lock = threading.Lock()
self.dict_firemap_msg = {uav: None for uav in self._known_uavs}
# type: ty.Mapping[str, rospy.Publisher]
self.pub_dict_firemap = {
uav: rospy.Publisher("/".join(
("uavs", serialization.ros_name_for(uav),
'wildfire_observed')),
WildfireMap,
queue_size=10)
for uav in self._known_uavs
}
self.current_saop_plan = None
self.sub_saop_plan = rospy.Subscriber("plan",
Plan,
self.on_saop_plan,
queue_size=10)
self.sub_saop_stop = rospy.Subscriber("stop",
StopCmd,
self.on_stop_cmd,
queue_size=10)
self.sub_saop_home = rospy.Subscriber("go_home",
HomeCmd,
self.on_home_cmd,
queue_size=10)
self.sub_wildfire_pred = rospy.Subscriber('wildfire_prediction',
PredictedWildfireMap,
self.on_wildifre_prediction,
queue_size=10)
self.sub_wildfire_current = rospy.Subscriber('wildfire',
WildfireMap,
self.on_wildfire_current,
queue_size=10)
self.sub_windspeed_dict = {
uav: rospy.Subscriber("/".join(
("uavs", serialization.ros_name_for(uav), 'windspeed')),
MeanWindStamped,
callback=self.on_wind_speed,
callback_args=uav,
queue_size=10)
for uav in self._known_uavs
}
rospy.Time.now() + rospy.Duration.from_sec(7),
PoseEuler(position=Point(536010.0, 4570910.0, 280.0),
orientation=Euler(.0, .0, .0)))
p = Plan(header=Header(stamp=rospy.Time.now()),
conf=p_conf,
trajectories=[Trajectory(conf=t_conf, maneuvers=[maneuver])])
self.pub_plan.publish(p)
def on_saop_plan(self, msg: Plan):
if self.supervision_state == SupervisorNodeState.Planning:
self.timer = rospy.Timer(rospy.Duration(secs=0, nsecs=500000000),
self.timer_callback,
oneshot=True)
if __name__ == '__main__':
try:
known_uavs = rospy.get_param("known_uavs")
uav_bases = {}
for uav in known_uavs:
uav_bases[uav] = rospy.get_param("/".join(
("uavs", serialization.ros_name_for(uav), "base")))
supnode = SupervisorNode(uav_bases)
while not rospy.is_shutdown():
rospy.spin()
except rospy.ROSInterruptException:
pass