Пример #1
0
    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)
Пример #2
0
 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