Perceptions.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// 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 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_