gps_sensor.hpp

// Copyright 2025 ros2_control development team
//
// 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 SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_
#define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_
 
#include <array>
#include <cstdint>
#include <limits>
#include <string>
#include <type_traits>
 
#include "rclcpp/version.h"
#include "semantic_components/semantic_component_interface.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
 
namespace semantic_components
{
 
enum class GPSSensorOption
{
  WithCovariance,
  WithoutCovariance
};
 
template <GPSSensorOption sensor_option>
class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
{
public:
#if RCLCPP_VERSION_MAJOR >= 28
  static constexpr int8_t STATUS_UNKNOWN = sensor_msgs::msg::NavSatStatus::STATUS_UNKNOWN;
  static constexpr uint16_t SERVICE_UNKNOWN = sensor_msgs::msg::NavSatStatus::SERVICE_UNKNOWN;
#else
  static constexpr int8_t STATUS_UNKNOWN = -2;
  static constexpr uint16_t SERVICE_UNKNOWN = 0;
#endif
 
  static_assert(
    sensor_option == GPSSensorOption::WithCovariance ||
      sensor_option == GPSSensorOption::WithoutCovariance,
    "Invalid GPSSensorOption");
  explicit GPSSensor(const std::string & name)
  : SemanticComponentInterface(
      name, {{name + "/" + "status"},
             {name + "/" + "service"},
             {name + "/" + "latitude"},
             {name + "/" + "longitude"},
             {name + "/" + "altitude"}})
  {
    if constexpr (sensor_option == GPSSensorOption::WithCovariance)
    {
      interface_names_.emplace_back(name + "/" + "latitude_covariance");
      interface_names_.emplace_back(name + "/" + "longitude_covariance");
      interface_names_.emplace_back(name + "/" + "altitude_covariance");
      state_interfaces_.reserve(state_interfaces_.capacity() + 3);
    }
  }
 
  int8_t get_status() const
  {
    const auto data = state_interfaces_[0].get().get_optional();
    if (data.has_value())
    {
      return static_cast<int8_t>(data.value());
    }
    return STATUS_UNKNOWN;
  }
 
  uint16_t get_service() const
  {
    const auto data = state_interfaces_[1].get().get_optional();
    if (data.has_value())
    {
      return static_cast<uint16_t>(data.value());
    }
    return SERVICE_UNKNOWN;
  }
 
  double get_latitude() const
  {
    const auto data = state_interfaces_[2].get().get_optional();
    if (data.has_value())
    {
      return data.value();
    }
    return std::numeric_limits<double>::quiet_NaN();
  }
 
  double get_longitude() const
  {
    const auto data = state_interfaces_[3].get().get_optional();
    if (data.has_value())
    {
      return data.value();
    }
    return std::numeric_limits<double>::quiet_NaN();
  }
 
  double get_altitude() const
  {
    const auto data = state_interfaces_[4].get().get_optional();
    if (data.has_value())
    {
      return data.value();
    }
    return std::numeric_limits<double>::quiet_NaN();
  }
 
  template <
    typename U = void,
    typename = std::enable_if_t<sensor_option == GPSSensorOption::WithCovariance, U>>
  const std::array<double, 9> & get_covariance()
  {
    const auto data_1 = state_interfaces_[5].get().get_optional();
    const auto data_2 = state_interfaces_[6].get().get_optional();
    const auto data_3 = state_interfaces_[7].get().get_optional();
    if (data_1.has_value())
    {
      covariance_[0] = data_1.value();
    }
    if (data_2.has_value())
    {
      covariance_[4] = data_2.value();
    }
    if (data_3.has_value())
    {
      covariance_[8] = data_3.value();
    }
    return covariance_;
  }
 
  bool get_values_as_message(sensor_msgs::msg::NavSatFix & message)
  {
    message.status.status = get_status();
    message.status.service = get_service();
    message.latitude = get_latitude();
    message.longitude = get_longitude();
    message.altitude = get_altitude();
 
    if constexpr (sensor_option == GPSSensorOption::WithCovariance)
    {
      message.position_covariance = get_covariance();
    }
 
    return true;
  }
 
private:
  std::array<double, 9> covariance_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
};
 
}  // namespace semantic_components
 
#endif  // SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_