generic_state.hpp

Go to the documentation of this file.

// Copyright 2021 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.
//
// Developed by Apex.AI, Inc.
 
 
#ifndef STATE_VECTOR__GENERIC_STATE_HPP_
#define STATE_VECTOR__GENERIC_STATE_HPP_
 
#include <common/type_traits.hpp>
#include <common/types.hpp>
#include <helper_functions/angle_utils.hpp>
#include <helper_functions/type_name.hpp>
#include <state_vector/common_variables.hpp>
#include <state_vector/visibility_control.hpp>
 
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic ignored "-Wuseless-cast"
#include <Eigen/Core>
#pragma GCC diagnostic pop
 
#include <tuple>
 
namespace autoware
{
namespace common
{
namespace state_vector
{
 
template<typename StateT>
struct is_state;
 
 
template<typename ScalarT, typename ... VariableTs>
class STATE_VECTOR_PUBLIC GenericState
{
  static_assert(
    common::type_traits::conjunction<is_variable<VariableTs>...>::value,
    "\n\nState can only be generated from variables, i.e. types that inherit from Variable.\n\n");
  static_assert(
    std::is_arithmetic<ScalarT>::value, "\n\nThe provided scalar type is not arithmetic.\n\n");
  static_assert(
    sizeof...(VariableTs) > 0, "\n\nCannot create state without variables.\n\n");
  // Hide this under private to make sure it is never ODR-used.
  constexpr static std::int32_t kSize = sizeof...(VariableTs);
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Variables = std::tuple<VariableTs...>;
  using Vector = Eigen::Matrix<ScalarT, kSize, 1>;
  using Matrix = Eigen::Matrix<ScalarT, kSize, kSize>;
  using Scalar = ScalarT;
 
  GenericState() = default;
 
  explicit GenericState(const Vector & vector)
  : m_state{vector} {}
 
  inline Vector & vector() noexcept {return m_state;}
  inline const Vector & vector() const noexcept {return m_state;}
 
  inline ScalarT & operator[](const Eigen::Index idx) {return m_state[idx];}
  inline const ScalarT & operator[](const Eigen::Index idx) const {return m_state[idx];}
 
  template<typename VariableT>
  inline ScalarT & at() noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  template<typename VariableT>
  inline ScalarT & at(const VariableT) noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  template<typename VariableT>
  inline const ScalarT & at() const noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
  template<typename VariableT>
  inline const ScalarT & at(const VariableT) const noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return m_state[common::type_traits::index<VariableT, Variables>::value];
  }
 
 
  template<typename OtherStateT>
  inline OtherStateT copy_into(OtherStateT other_state = OtherStateT{}) const noexcept
  {
    static_assert(
      is_state<OtherStateT>::value,
      "\n\nOtherStateT is not a GenericState.\n\n");
    auto copy_variables = [&other_state, this](auto variable) {
        other_state.at(variable) = this->at(variable);
      };
    using CommonVariablesTuple = typename autoware::common::type_traits::intersect<
      Variables, typename OtherStateT::Variables>::type;
    common::type_traits::visit(CommonVariablesTuple{}, copy_variables);
    return other_state;
  }
 
  template<typename VariableT>
  inline constexpr static Eigen::Index index_of() noexcept
  {
    static_assert(is_variable<VariableT>::value, "Type is not a Variable.");
    return common::type_traits::index<VariableT, Variables>::value;
  }
 
  inline constexpr static Eigen::Index size() noexcept {return kSize;}
 
  inline constexpr static Variables variables() noexcept{return Variables{};}
 
  friend constexpr bool operator==(const GenericState & lhs, const GenericState & rhs)
  {
    return lhs.m_state.isApprox(rhs.m_state);
  }
 
  friend std::ostream & operator<<(std::ostream & out, const GenericState & state)
  {
    out << "State:";
    auto print = [&out, &state](auto element) {
        out << "\n  " <<
          autoware::helper_functions::get_type_name(element) << ": " << state.at(element);
      };
    common::type_traits::visit(GenericState::variables(), print);
    return out;
  }
 
  inline GenericState & operator+=(const GenericState & rhs) noexcept
  {
    return this->operator+=(rhs.vector());
  }
 
  inline GenericState & operator+=(const Vector & rhs) noexcept
  {
    this->vector() += rhs;
    return *this;
  }
 
  inline GenericState & operator-=(const GenericState & rhs) noexcept
  {
    return this->operator-=(rhs.vector());
  }
 
  inline GenericState & operator-=(const Vector & rhs) noexcept
  {
    this->vector() -= rhs;
    return *this;
  }
 
 
  template<typename T>
  inline friend GenericState operator-(GenericState lhs, const T & rhs) noexcept
  {
    lhs -= rhs;
    return lhs;
  }
 
  template<typename T>
  inline friend GenericState operator+(GenericState lhs, const T & rhs) noexcept
  {
    lhs += rhs;
    return lhs;
  }
 
  constexpr void wrap_all_angles() noexcept
  {
    const auto wrap_angles = [this](auto variable) {
        using VariableT = decltype(variable);
        if (is_angle<VariableT>::value) {
          this->at(variable) = common::helper_functions::wrap_angle(this->at(variable));
        }
      };
    common::type_traits::visit(GenericState::variables(), wrap_angles);
  }
 
  constexpr friend GenericState wrap_all_angles(GenericState state) noexcept
  {
    state.wrap_all_angles();
    return state;
  }
 
  template<typename NewScalarT>
  GenericState<NewScalarT, VariableTs...> cast() const noexcept
  {
    return GenericState<NewScalarT, VariableTs...>{m_state.template cast<NewScalarT>()};
  }
 
private:
  Vector m_state{Vector::Zero()};
};
 
template<typename StateT>
struct STATE_VECTOR_PUBLIC is_state : public std::false_type {};
 
template<typename ScalarT, typename ... VariableTs>
struct STATE_VECTOR_PUBLIC is_state<GenericState<ScalarT, VariableTs...>>
  : public std::true_type {};
 
template<typename ... Ts>
using FloatState = GenericState<common::types::float32_t, Ts...>;
 
template<typename ... Ts>
using DoubleState = GenericState<common::types::float64_t, Ts...>;
 
}  // namespace state_vector
}  // namespace common
}  // namespace autoware
 
#endif  // STATE_VECTOR__GENERIC_STATE_HPP_