measurement_time_keeper.hpp

Go to the documentation of this file.

// Copyright 2020 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
 
#ifndef STATE_ESTIMATION_NODES__MEASUREMENT_TIME_KEEPER_HPP_
#define STATE_ESTIMATION_NODES__MEASUREMENT_TIME_KEEPER_HPP_
 
#include <common/types.hpp>
#include <state_estimation_nodes/measurement.hpp>
 
#include <algorithm>
#include <chrono>
#include <utility>
 
namespace autoware
{
namespace prediction
{
 
class MeasurementBasedTimeKeeper
{
public:
  MeasurementBasedTimeKeeper() = default;
 
  MeasurementBasedTimeKeeper(
    const GlobalTime & time_of_event,
    const MeasurementBasedTime & timestamp)
  : m_time_last_measurement_received{time_of_event}, m_last_measurement_timestamp{timestamp}
  {
    m_last_temporal_update = convert_to_measurement_time(time_of_event);
  }
 
  inline common::types::bool8_t is_initialized() const noexcept
  {
    return m_time_last_measurement_received.is_valid() && m_last_temporal_update.is_valid();
  }
 
  template<typename MeasurementT>
  inline void update_with_measurement(
    const GlobalTime & time_of_event,
    const MeasurementT & measurement)
  {
    m_time_last_measurement_received = time_of_event;
    m_last_measurement_timestamp = measurement.get_acquisition_time();
  }
 
  inline MeasurementBasedTime::duration time_since_last_temporal_update(
    const GlobalTime & time_point) const noexcept
  {
    return convert_to_measurement_time(time_point) - m_last_temporal_update;
  }
  inline MeasurementBasedTime::duration time_since_last_temporal_update(
    const MeasurementBasedTime & time_point) const noexcept
  {
    return time_point - m_last_temporal_update;
  }
 
  void increment_last_temporal_update_time(
    const MeasurementBasedTime::duration & time_since_last_update) noexcept
  {
    m_last_temporal_update += time_since_last_update;
  }
 
  MeasurementBasedTime latest_timestamp() const noexcept
  {
    return std::max(m_last_measurement_timestamp, m_last_temporal_update);
  }
 
private:
  inline MeasurementBasedTime convert_to_measurement_time(
    const GlobalTime & global_time) const noexcept
  {
    return m_last_measurement_timestamp + (global_time - m_time_last_measurement_received);
  }
 
  GlobalTime m_time_last_measurement_received{};
  MeasurementBasedTime m_last_measurement_timestamp{};
  MeasurementBasedTime m_last_temporal_update{};
};
 
 
}  // namespace prediction
}  // namespace autoware
 
#endif  // STATE_ESTIMATION_NODES__MEASUREMENT_TIME_KEEPER_HPP_