PointPerception.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__POINTPERCEPTION_HPP_
#define EASYNAV_SENSORS__TYPES__POINTPERCEPTION_HPP_
 
#include <string>
#include <vector>
#include <optional>
#include <mutex>
#include <memory>
#include <tuple>
#include <utility>
 
#include "tf2/LinearMath/Transform.hpp"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/PointIndices.h"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "easynav_sensors/types/Perceptions.hpp"
#include "easynav_common/CircularBuffer.hpp"
#include "easynav_common/RTTFBuffer.hpp"
 
namespace std
{

template<>
struct hash<std::tuple<int, int, int>>
{
  std::size_t operator()(const std::tuple<int, int, int> & key) const
  {
    std::size_t h1 = std::hash<int>()(std::get<0>(key));
    std::size_t h2 = std::hash<int>()(std::get<1>(key));
    std::size_t h3 = std::hash<int>()(std::get<2>(key));
    return h1 ^ (h2 << 1) ^ (h3 << 2);
  }
};
 
}  // namespace std
 
namespace easynav
{
 
struct PointPerceptionBufferType
{
  pcl::PointCloud<pcl::PointXYZ> data;
  std::string frame;
  rclcpp::Time stamp;
};

class PointPerception : public PerceptionBase
{
public:
  static constexpr std::string_view default_group_ = "points";

  static inline bool supports_msg_type(std::string_view t)
  {
    return t == "sensor_msgs/msg/LaserScan" ||
           t == "sensor_msgs/msg/PointCloud2";
  }
 
  PointPerception()
  {
    [[maybe_unused]] static const bool _ = [] {
        ::easynav::NavState::register_printer<PointPerception>(
          [](const PointPerception & perception) {
            std::ostringstream ret;
            ret << "{ " << perception.stamp.seconds()
                << " } PointPerception with " << perception.data.size()
                << " points in frame [" << perception.frame_id
                << "] with ts " << perception.stamp.seconds() << "\n";
            return ret.str();
          });
        return true;
      }();
  }
 
  PointPerception(const PointPerception & other)
  {
    std::lock_guard<std::mutex> lock(other.mutex_);
    stamp = other.stamp;
    frame_id = other.frame_id;
    valid = other.valid;
    new_data = other.new_data;
 
    data = other.data;
    pending_available_ = other.pending_available_;
    pending_cloud_ = other.pending_cloud_;
    pending_frame_ = other.pending_frame_;
    pending_stamp_ = other.pending_stamp_;
    buffer = other.buffer;
  }
 
  PointPerception & operator=(const PointPerception & other)
  {
    if (this == &other) {
      return *this;
    }
 
    std::scoped_lock lock(mutex_, other.mutex_);
    stamp = other.stamp;
    frame_id = other.frame_id;
    valid = other.valid;
    new_data = other.new_data;
 
    data = other.data;
    pending_available_ = other.pending_available_;
    pending_cloud_ = other.pending_cloud_;
    pending_frame_ = other.pending_frame_;
    pending_stamp_ = other.pending_stamp_;
    buffer = other.buffer;
 
    return *this;
  }

  pcl::PointCloud<pcl::PointXYZ> data;
 
  bool pending_available_{false};
  pcl::PointCloud<pcl::PointXYZ> pending_cloud_;
  std::string pending_frame_;
  rclcpp::Time pending_stamp_;

  void set_pending_cloud(
    pcl::PointCloud<pcl::PointXYZ> && cloud,
    std::string && frame,
    const rclcpp::Time & stamp)
  {
    std::lock_guard<std::mutex> lock(mutex_);
    pending_cloud_ = std::move(cloud);
    pending_frame_ = std::move(frame);
    pending_stamp_ = stamp;
    pending_available_ = true;
  }
 

  void resize(std::size_t size)
  {
    data.points.resize(size);
  }
 
  void integrate_pending_perceptions()
  {
    std::lock_guard<std::mutex> lock(mutex_);
    // Access TF buffer singleton (already initialized somewhere with a clock)
    auto tf_buffer_ptr = RTTFBuffer::getInstance();
    auto & tf_buffer = *tf_buffer_ptr;
    const auto tf_info = tf_buffer.get_tf_info();
    const std::string & robot_frame = tf_info.robot_frame;
 
    // ------------------------------------------------------------------
    // 1. Push pending perception into the circular buffer exactly once.
    // ------------------------------------------------------------------
    if (pending_available_) {
      PointPerceptionBufferType pending_item;
      pending_item.data = std::move(pending_cloud_);  // avoid deep copy
      pending_item.frame = pending_frame_;
      pending_item.stamp = pending_stamp_;
 
      buffer.push(std::move(pending_item));
      pending_available_ = false;
    }
 
    const std::size_t count = buffer.size();
    if (count == 0) {
      // No candidates at all: keep current visible state as is.
      return;
    }
 
    // ------------------------------------------------------------------
    // 2. Drain the circular buffer into a temporary vector so we can
    //    inspect all items and then rebuild the buffer.
    // ------------------------------------------------------------------
    std::vector<PointPerceptionBufferType> items;
    items.reserve(count);
 
    for (std::size_t i = 0; i < count; ++i) {
      PointPerceptionBufferType item;
      if (!buffer.pop(item)) {
        break;  // Defensive guard if pop() fails unexpectedly.
      }
      items.push_back(std::move(item));
    }
 
    if (items.empty()) {
      // Nothing recovered from buffer: keep visible state untouched.
      return;
    }
 
    // ------------------------------------------------------------------
    // 3. Find indices:
    //    - newest_idx: newest perception by timestamp (regardless of TF),
    //    - newest_valid_idx: newest perception that has a valid TF.
    //
    //    IMPORTANT: store only indices to avoid copying point clouds
    //    during the scan.
    // ------------------------------------------------------------------
    std::optional<std::size_t> newest_idx;
    std::optional<std::size_t> newest_valid_idx;
 
    for (std::size_t i = 0; i < items.size(); ++i) {
      const auto & item = items[i];
 
      // Track newest item overall (used when no TF is valid).
      if (!newest_idx || item.stamp > items[*newest_idx].stamp) {
        newest_idx = i;
      }
 
      bool has_tf = false;
      try {
        has_tf = tf_buffer.canTransform(
          robot_frame,
          item.frame,
          tf2_ros::fromMsg(item.stamp));
      } catch (...) {
        // Any TF exception is treated as "no valid TF" for this item.
        has_tf = false;
      }
 
      if (has_tf) {
        if (!newest_valid_idx || item.stamp > items[*newest_valid_idx].stamp) {
          newest_valid_idx = i;
        }
      }
    }
 
    // ------------------------------------------------------------------
    // 4. Update visible state BEFORE moving items back into the buffer.
    //    This guarantees that `data` corresponds to:
    //      - the newest TF-valid item if any exists, otherwise
    //      - the newest item overall.
    // ------------------------------------------------------------------
    if (newest_valid_idx) {
      const auto & sel = items[*newest_valid_idx];
      data = sel.data;        // single deep copy (intentional)
      frame_id = sel.frame;
      stamp = sel.stamp;
      valid = true;           // "valid" means "usable / not too old", not "TF ok"
      new_data = true;
    } else if (newest_idx) {
      const auto & sel = items[*newest_idx];
      data = sel.data;        // single deep copy (intentional)
      frame_id = sel.frame;
      stamp = sel.stamp;
      valid = true;
      new_data = true;
    } else {
      // Defensive: should not happen because items is non-empty.
      return;
    }
 
    // ------------------------------------------------------------------
    // 5. Rebuild the circular buffer from scratch (move-only, no copies).
    // ------------------------------------------------------------------
    buffer.clear();
 
    if (newest_valid_idx) {
      // Keep the newest TF-valid item and any newer items (even if TF is not yet available).
      const rclcpp::Time cutoff_stamp = items[*newest_valid_idx].stamp;
 
      for (auto & item : items) {
        if (item.stamp >= cutoff_stamp) {
          buffer.push(std::move(item));
        }
      }
    } else {
      // No TF-valid items: keep everything in case TF arrives later.
      for (auto & item : items) {
        buffer.push(std::move(item));
      }
    }
  }
 
protected:
  mutable std::mutex mutex_;
  CircularBuffer<PointPerceptionBufferType> buffer{10};
};

class PointPerceptionHandler : public PerceptionHandler
{
public:
  void on_initialize() override;

  bool cycle_rt([[maybe_unused]] std::shared_ptr<NavState> nav_state) override;

  std::shared_ptr<PerceptionBase> get_perception() const override {return perception_data_;}
 
private:
  std::shared_ptr<PointPerception> perception_data_ {nullptr};

  rclcpp::SubscriptionBase::SharedPtr perception_sub_;
};

void convert(const sensor_msgs::msg::LaserScan & scan, pcl::PointCloud<pcl::PointXYZ> & pc);

sensor_msgs::msg::PointCloud2 perception_to_rosmsg(const PointPerception & perception);

sensor_msgs::msg::PointCloud2 points_to_rosmsg(const pcl::PointCloud<pcl::PointXYZ> & points);

using PointPerceptions =
  std::vector<std::shared_ptr<PointPerception>>;

rclcpp::Time get_latest_point_perceptions_stamp(const PointPerceptions & perceptions);

class PointPerceptionsOpsView
{
public:
  struct VoxelKey
  {
    int x, y, z;

    bool operator==(const VoxelKey & other) const
    {
      return x == other.x && y == other.y && z == other.z;
    }
  };

  struct VoxelKeyHash
  {
    std::size_t operator()(const VoxelKey & key) const
    {
      std::size_t h1 = std::hash<int>{}(key.x);
      std::size_t h2 = std::hash<int>{}(key.y);
      std::size_t h3 = std::hash<int>{}(key.z);
      return h1 ^ (h2 << 1) ^ (h3 << 2);
    }
  };

  explicit PointPerceptionsOpsView(const PointPerceptions & perceptions);

  explicit PointPerceptionsOpsView(const PointPerception & perception);

  explicit PointPerceptionsOpsView(PointPerceptions && perceptions);
 
  PointPerceptionsOpsView(const PointPerceptionsOpsView &) = delete;
  PointPerceptionsOpsView & operator=(const PointPerceptionsOpsView &) = delete;
 
  PointPerceptionsOpsView(PointPerceptionsOpsView &&) = default;

  PointPerceptionsOpsView & filter(
    const std::vector<double> & min_bounds,
    const std::vector<double> & max_bounds,
    bool lazy_post_fuse = true);

  PointPerceptionsOpsView & downsample(double resolution);

  PointPerceptionsOpsView &
  collapse(const std::vector<double> & collapse_dims, bool lazy = true);

  pcl::PointCloud<pcl::PointXYZ> as_points() const;

  PointPerceptionsOpsView & fuse(const std::string & target_frame, bool exact_time = false);

  PointPerceptionsOpsView & fuse(
    const std::string & target_frame,
    rclcpp::Time & stamp,
    bool exact_time = false);

  PointPerceptionsOpsView &
  add(
    const pcl::PointCloud<pcl::PointXYZ> points,
    const std::string & frame,
    rclcpp::Time stamp);

  const PointPerceptions & get_perceptions() const {return perceptions_;}

  rclcpp::Time get_latest_stamp() const;
 
private:
  std::optional<PointPerceptions> owned_;      
  const PointPerceptions & perceptions_;       
  std::vector<pcl::PointIndices> indices_;     
 
  // Lazy fusion state
  bool has_target_frame_ {false};
  std::string target_frame_;
  std::vector<tf2::Transform> tf_transforms_;
  std::vector<bool> tf_valid_;
 
  // Lazy collapse state
  bool collapse_x_ {false};                    
  bool collapse_y_ {false};                    
  bool collapse_z_ {false};                    
  float collapse_val_x_ {0.0f};                
  float collapse_val_y_ {0.0f};                
  float collapse_val_z_ {0.0f};                
 
  bool has_post_filter_ {false};
  double post_min_[3] {0.0, 0.0, 0.0};
  double post_max_[3] {0.0, 0.0, 0.0};
  bool use_post_min_[3] {false, false, false};
  bool use_post_max_[3] {false, false, false};
};
 
}  // namespace easynav
 
#endif  // EASYNAV_SENSORS__TYPES__POINTPERCEPTION_HPP_