Branch data     Line data    Source code

       1                 :            : // Copyright 2020 the Autoware Foundation
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : #include <localization_common/initialization.hpp>
      18                 :            : #include <time_utils/time_utils.hpp>
      19                 :            : #include <string>
      20                 :            : 
      21                 :            : namespace autoware
      22                 :            : {
      23                 :            : namespace localization
      24                 :            : {
      25                 :            : namespace localization_common
      26                 :            : {
      27                 :          6 : geometry_msgs::msg::TransformStamped BestEffortInitializer::extrapolate(
      28                 :            :   const tf2::BufferCore & tf_graph, tf2::TimePoint time_point,
      29                 :            :   const std::string & target_frame, const std::string & source_frame)
      30                 :            : {
      31                 :            :   (void) time_point;
      32                 :          6 :   const auto ret = tf_graph.lookupTransform(target_frame, source_frame, tf2::TimePointZero);
      33         [ +  + ]:          6 :   if (time_utils::from_message(ret.header.stamp) > time_point) {
      34                 :            :     // It's older than the oldest because if it was within [oldest_available, newest_available]
      35                 :            :     // we wouldn't be in this function.
      36                 :            :     throw std::domain_error(
      37                 :            :             "BestEffortInitializer: Backwards extrapolation is not supported."
      38                 :            :             "Initialization timepoint is older than the oldest available "
      39         [ +  - ]:          1 :             "transform in the transform graph.");
      40                 :            :   }
      41                 :          5 :   return ret;
      42                 :            : }
      43                 :            : 
      44                 :            : }  // namespace localization_common
      45                 :            : }  // namespace localization
      46                 :            : }  // namespace autoware