localizer_base.hpp

Go to the documentation of this file.

// Copyright 2019 the Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
 
#ifndef LOCALIZATION_COMMON__LOCALIZER_BASE_HPP_
#define LOCALIZATION_COMMON__LOCALIZER_BASE_HPP_
 
#include <localization_common/visibility_control.hpp>
#include <tf2/buffer_core.h>
#include <geometry_msgs/msg/transform.hpp>
#include <localization_common/initialization.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <optimization/optimizer_options.hpp>
#include <string>
 
#include "common/types.hpp"
 
namespace autoware
{
namespace localization
{
namespace localization_common
{
using Real = autoware::common::types::float64_t;
 
class LOCALIZATION_COMMON_PUBLIC OptimizedRegistrationSummary
{
public:
  using OptimizationSummary = common::optimization::OptimizationSummary;
  explicit OptimizedRegistrationSummary(const OptimizationSummary & opt_summary);
 
  OptimizationSummary optimization_summary() const;
 
private:
  OptimizationSummary m_optimization_summary;
};
 
template<typename InputMsgT, typename MapMsgT, typename RegistrationSummaryT>
class LOCALIZATION_COMMON_PUBLIC RelativeLocalizerBase
{
public:
  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
  using Transform = geometry_msgs::msg::TransformStamped;
  using RegistrationSummary = RegistrationSummaryT;
  using InputMsg = InputMsgT;
  using MapMsg = MapMsgT;
  RegistrationSummaryT register_measurement(
    const InputMsgT & msg, const Transform & transform_initial,
    PoseWithCovarianceStamped & pose_out)
  {
    if (!m_map_valid) {
      on_register_without_map();
    }
    return register_measurement_impl(msg, transform_initial, pose_out);
  }
 
  void set_map(const MapMsgT & msg)
  {
    try {
      set_map_impl(msg);
      set_map_valid();
    } catch (...) {
      set_map_invalid();
      on_bad_map(std::current_exception());
    }
  }
 
  void insert_to_map(const MapMsgT & msg)
  {
    try {
      insert_to_map_impl(msg);
    } catch (...) {
      on_bad_map_insertion(std::current_exception());
    }
  }
 
  virtual const std::string & map_frame_id() const noexcept = 0;
 
  virtual std::chrono::system_clock::time_point map_stamp() const noexcept = 0;
 
  bool map_valid() const noexcept
  {
    return m_map_valid;
  }
 
  // Deleting move assignment deletes copy and move assignment operators/constructors
  // See `Rule of DesDeMovA`: https://safecpp.com/2019/07/01/initial.html
  RelativeLocalizerBase & operator=(RelativeLocalizerBase && other) = delete;
 
  virtual ~RelativeLocalizerBase() = default;
 
protected:
  virtual void set_map_impl(const MapMsgT & msg) = 0;
 
  virtual void insert_to_map_impl(const MapMsgT &)
  {
    throw std::runtime_error("RelativeLocalizerBase: map insertion is not implemented.");
  }
 
  virtual void on_bad_map(std::exception_ptr eptr)
  {
    if (eptr) {
      std::rethrow_exception(eptr);
    }
  }
  virtual void on_bad_map_insertion(std::exception_ptr eptr)
  {
    if (eptr) {
      std::rethrow_exception(eptr);
    }
  }
 
  virtual RegistrationSummaryT register_measurement_impl(
    const InputMsgT & msg,
    const Transform & transform_initial, PoseWithCovarianceStamped & pose_out) = 0;
 
  void set_map_valid() noexcept
  {
    m_map_valid = true;
  }
 
  void set_map_invalid() noexcept
  {
    m_map_valid = false;
  }
 
  virtual void on_register_without_map()
  {
    throw std::logic_error(
            "RelativeLocalizerBase: Cannot register a measurement without a valid"
            " map. Either no valid map is set yet or an invalid map had been "
            "attempted to be set.");
  }
 
private:
  bool m_map_valid{false};
};
}  // namespace localization_common
}  // namespace localization
}  // namespace autoware
 
#endif  // LOCALIZATION_COMMON__LOCALIZER_BASE_HPP_