def propagate_and_set_fire_front(
self, origin, origin_time: rospy.Time, perimeter_time: rospy.Time,
environment: fire_rs.firemodel.propagation.Environment):
origin_pos = Timed2DPointStamped(header=Header(stamp=rospy.Time.now()),
x=origin[0],
y=origin[1],
t=origin_time)
fireprop = fire_rs.firemodel.propagation.propagate_from_points(
environment,
fire_rs.firemodel.propagation.TimedPoint(*origin,
origin_time.to_sec()))
firemap = fireprop.ignitions()
array, cells, contours = fire_rs.geodata.wildfire._compute_perimeter(
firemap, perimeter_time.to_sec())
# Publish ignition points
# initial alarm
self.pub_w.publish(origin_pos)
self.pub_w.publish(origin_pos)
rospy.loginfo(origin_pos)
wildfire_message = WildfireMap(
header=rospy.Header(stamp=rospy.Time.now()),
raster=serialization.raster_msg_from_geodata(
firemap.clone(data_array=array, dtype=firemap.data.dtype),
'ignition'))
self.pub_wildfire_obs.publish(wildfire_message)
rospy.loginfo(wildfire_message)
def lookup_trajectory(
self,
parent_frame: str,
child_frame: str,
start_time: rospy.Time,
end_time: rospy.Time,
lookup_frequency: float = SETTINGS.tf_cache_lookup_frequency
) -> PoseTrajectory3D:
"""
Look up the trajectory of a transform chain from the cache's TF buffer.
:param parent_frame, child_frame: TF transform frame IDs
:param start_time, end_time: expected start and end time of the
trajectory in the buffer
:param lookup_frequency: frequency of TF lookups between start and end
time, in Hz.
"""
stamps, xyz, quat = [], [], []
step = rospy.Duration.from_sec(1. / lookup_frequency)
# Static TF have zero timestamp in the buffer, which will be lower
# than the bag start time. Looking up a static TF is a valid request,
# so this should be possible.
attempt_single_static_lookup = end_time.to_sec() == 0.
# Look up the transforms of the trajectory in reverse order:
while end_time >= start_time or attempt_single_static_lookup:
try:
tf = self.buffer.lookup_transform_core(parent_frame,
child_frame, end_time)
except tf2_py.ExtrapolationException:
break
stamps.append(tf.header.stamp.to_sec())
x, q = _get_xyz_quat_from_transform_stamped(tf)
xyz.append(x)
quat.append(q)
if attempt_single_static_lookup:
break
end_time = end_time - step
# Flip the data order again for the final trajectory.
trajectory = PoseTrajectory3D(np.flipud(xyz), np.flipud(quat),
np.flipud(stamps))
trajectory.meta = {
"frame_id": parent_frame,
"child_frame_id": child_frame
}
return trajectory
