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_SENSORS__TYPES__PERCEPTIONS_HPP_
#define EASYNAV_SENSORS__TYPES__PERCEPTIONS_HPP_
 
#include <string>
#include <memory>
#include <utility>
#include <vector>
 
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "easynav_common/types/NavState.hpp"
 
namespace easynav
{
 
// Forward declaration needed by PerceptionPtr.
class PerceptionHandler;

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>;
 

template<typename T = PerceptionBase>
inline std::vector<std::shared_ptr<T>>
get_perceptions(const std::vector<PerceptionBasePtr> & 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 & perception : src) {
    if (!perception) {continue;}
    // Homogeneous by derived type: include only successful casts
    if (auto p = std::dynamic_pointer_cast<T>(perception)) {
      out.push_back(std::move(p));
    }
  }
  return out;
}
 

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

  void initialize(
    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> parent_node,
    const rclcpp::CallbackGroup::SharedPtr realtime_cbg,
    const std::string & sensor_name)
  {
    parent_node_ = parent_node;
    realtime_cbg_ = realtime_cbg;
    sensor_name_ = sensor_name;
    on_initialize();
  }

  virtual void on_initialize() {}

  virtual bool cycle_rt([[maybe_unused]] std::shared_ptr<NavState> nav_state) {return false;}
 

  const std::string & get_sensor_name() const {return sensor_name_;}

  virtual std::shared_ptr<PerceptionBase> get_perception() const {return nullptr;}
 
protected:
  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node() const {return parent_node_.lock();}

  rclcpp::CallbackGroup::SharedPtr get_realtime_cbg() const {return realtime_cbg_;}

  std::weak_ptr<rclcpp_lifecycle::LifecycleNode> parent_node_;

  rclcpp::CallbackGroup::SharedPtr realtime_cbg_;

  std::string sensor_name_;
};
 
}  // namespace easynav
 
#endif  // EASYNAV_SENSORS__TYPES__PERCEPTIONS_HPP_