Perceptions.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// licensed under the GNU General Public License v3.0.
// See <http://www.gnu.org/licenses/> for details.
//
// Easy Navigation program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

 
#ifndef EASYNAV_COMMON_TYPES__PERCEPTIONS_HPP_
#define EASYNAV_COMMON_TYPES__PERCEPTIONS_HPP_
 
#include <string>
 
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
namespace easynav
{

class PerceptionBase
{
public:
  virtual ~PerceptionBase() = default;

  rclcpp::Time stamp;

  std::string frame_id;

  bool valid = false;

  bool new_data = false;
};

using PerceptionBasePtr = std::shared_ptr<PerceptionBase>;

struct PerceptionPtr
{
  PerceptionBasePtr perception;

  rclcpp::SubscriptionBase::SharedPtr subscription;
};

template<typename T = PerceptionBase>
inline std::vector<std::shared_ptr<T>>
get_perceptions(const std::vector<PerceptionPtr> & src)
{
  static_assert(std::is_base_of_v<PerceptionBase, T>,
                "T must inherit from PerceptionBase");
 
  std::vector<std::shared_ptr<T>> out;
  out.reserve(src.size());
 
  for (const auto & h : src) {
    if (!h.perception) {continue;}
 
    if constexpr (std::is_same_v<T, PerceptionBase>) {
      // Heterogeneous: no cast needed
      out.push_back(h.perception);
    } else {
      // Homogeneous by derived type: include only successful casts
      if (auto p = std::dynamic_pointer_cast<T>(h.perception)) {
        out.push_back(std::move(p));
      }
    }
  }
  return out;
}

class PerceptionHandler
{
public:
  virtual ~PerceptionHandler() = default;

  virtual std::shared_ptr<PerceptionBase> create(const std::string & sensor_id) = 0;

  virtual rclcpp::SubscriptionBase::SharedPtr create_subscription(
    rclcpp_lifecycle::LifecycleNode & node,
    const std::string & topic,
    const std::string & type,
    std::shared_ptr<PerceptionBase> target,
    rclcpp::CallbackGroup::SharedPtr cb_group) = 0;

  virtual std::string group() const = 0;
};
 
}  // namespace easynav
 
#endif  // EASYNAV_COMMON_TYPES__PERCEPTIONS_HPP_