measurement_conversion.hpp

Go to the documentation of this file.

// Copyright 2021 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
 
 
#ifndef MEASUREMENT_CONVERSION__MEASUREMENT_CONVERSION_HPP_
#define MEASUREMENT_CONVERSION__MEASUREMENT_CONVERSION_HPP_
 
#include <autoware_auto_geometry_msgs/msg/relative_position_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
#include <measurement_conversion/measurement_typedefs.hpp>
#include <measurement_conversion/visibility_control.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/time.hpp>
#include <time_utils/time_utils.hpp>
 
#include <Eigen/Geometry>
 
namespace autoware
{
namespace common
{
namespace state_estimation
{
 
namespace detail
{
inline const geometry_msgs::msg::PoseWithCovariance & unstamp(
  const geometry_msgs::msg::PoseWithCovarianceStamped & msg)
{
  return msg.pose;
}
inline const autoware_auto_geometry_msgs::msg::RelativePositionWithCovarianceStamped & unstamp(
  const autoware_auto_geometry_msgs::msg::RelativePositionWithCovarianceStamped & msg) {return msg;}
 
}  // namespace detail
 
template<typename MeasurementT>
struct MEASUREMENT_CONVERSION_PUBLIC convert_to
{
  static_assert(
    autoware::common::type_traits::impossible_branch<MeasurementT>(),
    "This struct should always have a specialization.");
};
 
template<typename MeasurementT>
struct MEASUREMENT_CONVERSION_PUBLIC convert_to<Stamped<MeasurementT>>
{
  template<typename MsgT>
  static Stamped<MeasurementT> from(const MsgT & msg)
  {
    return Stamped<MeasurementT>{
      time_utils::from_message(msg.header.stamp),
      convert_to<MeasurementT>::from(detail::unstamp(msg))
    };
  }
};
 
template<>
struct MEASUREMENT_CONVERSION_PUBLIC convert_to<PoseMeasurementXYZRPY64>
{
  using MatrixT = PoseMeasurementXYZRPY64::State::Matrix;
  static PoseMeasurementXYZRPY64 from(const geometry_msgs::msg::PoseWithCovariance & msg);
  static bool variance_check(MatrixT & covariance);
  static double noise_add_to_variance;
};
 
template<>
struct MEASUREMENT_CONVERSION_PUBLIC convert_to<PoseMeasurementXYZ64>
{
  static PoseMeasurementXYZ64 from(
    const autoware_auto_geometry_msgs::msg::RelativePositionWithCovarianceStamped & msg);
};
 
}  // namespace state_estimation
}  // namespace common
}  // namespace autoware
 
 
#endif  // MEASUREMENT_CONVERSION__MEASUREMENT_CONVERSION_HPP_