point_cloud_fusion::PointCloudFusion Class Reference

PointCloudFusion class. More...

#include <point_cloud_fusion.hpp>

Inheritance diagram for point_cloud_fusion::PointCloudFusion:

Classes
struct	FusionTiming
	Timing metadata for one synchronized fusion batch. More...

Public Member Functions
	PointCloudFusion (const rclcpp::NodeOptions &options)
	Constructor.

Private Types
enum class	OutputStampMode { Latest , Earliest , Mean , Input0 }
using	PointCloudMsg = sensor_msgs::msg::PointCloud2

Private Member Functions
template<typename T >
void	declareAndLoadParameter (const std::string &name, T &param, const std::string &description, const bool add_to_auto_reconfigurable_params=true, const bool is_required=false, const bool read_only=false, const std::optional< double > &from_value=std::nullopt, const std::optional< double > &to_value=std::nullopt, const std::optional< double > &step_value=std::nullopt, const std::string &additional_constraints="")
	Declares and loads a ROS parameter.
rcl_interfaces::msg::SetParametersResult	parametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Validate and apply dynamic parameter updates.
void	setup ()
	Sets up subscribers, publishers, etc. to configure the node.
void	handleSynchronizedPointClouds (const std::vector< sensor_msgs::msg::PointCloud2::ConstSharedPtr > &msgs)
	Process synchronized point clouds.
template<std::size_t N>
void	setupSynchronizer ()
	Create the approximate-time synchronizer for a fixed number of input topics.
bool	collectTimingInfo (const std::vector< PointCloudMsg::ConstSharedPtr > &msgs, FusionTiming &timing) const
	Collect timestamp statistics for synchronized input clouds.
PointCloudMsg::UniquePtr	fusePointCloudBatch (const std::vector< PointCloudMsg::ConstSharedPtr > &msgs, const FusionTiming &timing, std::size_t &valid_point_count) const
	Fuse a synchronized point-cloud batch using the CPU path.
void	publishFusedCloud (PointCloudMsg::UniquePtr cloud, const FusionTiming &timing, std::size_t input_count, std::size_t total_points, std::chrono::steady_clock::time_point callback_start, std::chrono::steady_clock::time_point processing_start, std::chrono::steady_clock::time_point processing_end, const char *event_name)
	Publish a fused point cloud and emit tracing/timing diagnostics.
void	configureOutputStampMode (const std::string &mode)
	Parse and store the configured output timestamp mode.
void	validateInputTopicsParameter () const
	Validate that the configured input topic list is usable.
void	validateRangeLimits ()
	Validate configured XYZ range limits and disable filtering when invalid.

Private Attributes
double	max_time_diff_sec_ = 0.05
	ROS parameters.
double	age_penalty_ = 0.1
int64_t	sync_queue_size_ = 3
int64_t	output_queue_size_ = 10
int64_t	fixed_points_per_input_cloud_ = 0
bool	range_limits_enable_ = false
double	range_limits_x_min_ = -1000.0
double	range_limits_x_max_ = 1000.0
double	range_limits_y_min_ = -1000.0
double	range_limits_y_max_ = 1000.0
double	range_limits_z_min_ = -20.0
double	range_limits_z_max_ = 20.0
bool	use_cuda_ = true
OutputStampMode	output_stamp_mode_ = OutputStampMode::Earliest
std::string	output_stamp_mode_param_ = "earliest"
std::string	target_frame_ = "base_link"
std::vector< std::string >	output_fields_
std::vector< std::string >	input_topics_
std::vector< std::string >	input_transport_hints_
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > >	auto_reconfigurable_params_
	Auto-reconfigurable parameters for dynamic reconfiguration.
std::shared_mutex	config_mutex_
std::vector< std::shared_ptr< point_cloud_transport::SubscriberFilter > >	cloud_subscribers_
std::vector< rclcpp::CallbackGroup::SharedPtr >	cloud_subscriber_callback_groups_
std::shared_ptr< void >	synchronizer_
OnSetParametersCallbackHandle::SharedPtr	parameters_callback_
	Callback handle for dynamic parameter reconfiguration.
std::shared_ptr< point_cloud_transport::Publisher >	cloud_publisher_
	Publisher.
std::shared_ptr< tf2_ros::Buffer >	tf_buffer_
	TF2 buffer and transform listener.
std::shared_ptr< tf2_ros::TransformListener >	tf_listener_
rclcpp::TimerBase::SharedPtr	setup_timer_
	Timer to delay setup.

Static Private Attributes
static constexpr int32_t	kMinSyncQueueSize = 1
static constexpr int32_t	kMaxSyncQueueSize = 1000
static constexpr int32_t	kStepSizeSyncQueueSize = 1
static constexpr int32_t	kMinOutputQueueSize = 1
static constexpr int32_t	kMaxOutputQueueSize = 1000
static constexpr int32_t	kStepSizeOutputQueueSize = 1
static constexpr int64_t	kMinFixedPointsPerInputCloud = 0
static constexpr int64_t	kMaxFixedPointsPerInputCloud = 10000000
static constexpr int64_t	kStepSizeFixedPointsPerInputCloud = 100
static constexpr std::size_t	kMaxInputTopics = 9
static constexpr const char *	kDefaultTransportHint = "raw"
static constexpr const char *	kAllowedOutputStampModes = "latest, earliest, mean, input0"
static constexpr double	kMinRangeXY = -1000.0
static constexpr double	kMaxRangeXY = 1000.0
static constexpr double	kMinRangeZ = -20.0
static constexpr double	kMaxRangeZ = 20.0

Detailed Description

PointCloudFusion class.

Definition at line 45 of file point_cloud_fusion.hpp.

Member Typedef Documentation

PointCloudMsg

using point_cloud_fusion::PointCloudFusion::PointCloudMsg = sensor_msgs::msg::PointCloud2

private

Definition at line 111 of file point_cloud_fusion.hpp.

Member Enumeration Documentation

OutputStampMode

enum class point_cloud_fusion::PointCloudFusion::OutputStampMode

strongprivate

Enumerator
Latest
Earliest
Mean
Input0

Definition at line 176 of file point_cloud_fusion.hpp.

{ Latest, Earliest, Mean, Input0 };

Constructor & Destructor Documentation

PointCloudFusion()

point_cloud_fusion::PointCloudFusion::PointCloudFusion ( const rclcpp::NodeOptions & options )

explicit

Constructor.

Parameters

options

node options

Definition at line 114 of file point_cloud_fusion.cpp.

                                                                   : Node("point_cloud_fusion", options) {
  this->declareAndLoadParameter("target_frame", target_frame_,                   // name
                                "Frame into which all input point clouds are transformed before fusion",  // description
                                false,                                           // add_to_auto_reconfigurable_params
                                true,                                            // is_required
                                true,                                            // read_only
                                std::nullopt, std::nullopt, std::nullopt,        // from_value, to_value, step_value
                                "Must be set.");                                 // additional_constraints
  this->declareAndLoadParameter("input_topics", input_topics_,                   // name
                                "Point-cloud topics to fuse",                    // description
                                false,                                           // add_to_auto_reconfigurable_params
                                true,                                            // is_required
                                true,                                            // read_only
                                std::nullopt, std::nullopt, std::nullopt,        // from_value, to_value, step_value
                                "Must configure between 1 and " + std::to_string(kMaxInputTopics) + " topics");
  validateInputTopicsParameter();
  this->declareAndLoadParameter("input_transport_hints", input_transport_hints_, // name
                                "Transport hint for each input topic; unspecified entries use raw",  // description
                                false,                                           // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                true,                                            // read_only
                                std::nullopt, std::nullopt, std::nullopt,        // from_value, to_value, step_value
                                "Length must be zero or match input_topics; unspecified entries default to '" +
                                    std::string(kDefaultTransportHint) + "'.");  // additional_constraints
  this->declareAndLoadParameter("sync_queue_size", sync_queue_size_,             // name
                                "Queue depth for approximate-time synchronization",  // description
                                false,                                           // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                true,                                            // read_only
                                kMinSyncQueueSize,                               // from_value
                                kMaxSyncQueueSize,                               // to_value
                                kStepSizeSyncQueueSize,                          // step_value
                                std::string("Must be >= ") + std::to_string(kMinSyncQueueSize));  // additional_constraints
  this->declareAndLoadParameter("output_queue_size", output_queue_size_,         // name
                                "Queue depth for the fused output publisher",    // description
                                false,                                           // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                true,                                            // read_only
                                kMinOutputQueueSize,                             // from_value
                                kMaxOutputQueueSize,                             // to_value
                                kStepSizeOutputQueueSize,                        // step_value
                                std::string("Must be >= ") + std::to_string(kMinOutputQueueSize)); // additional_constraints
  this->declareAndLoadParameter(
      "output_fields", output_fields_,                                           // name
      "Fields retained in the fused output; an empty list retains all input fields",  // description
      true,                                                                      // add_to_auto_reconfigurable_params
      false,                                                                     // is_required
      false,                                                                     // read_only
      std::nullopt, std::nullopt, std::nullopt,                                  // from_value, to_value, step_value
      "Typical fields include: x, y, z, intensity, t, reflectivity, ring, ambient, range.");  // additional_constraints
  this->declareAndLoadParameter("output_stamp_mode", output_stamp_mode_param_,   // name
                                "Fused timestamp selection: earliest, latest, mean, or input0",
                                false,                                           // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                true,                                            // read_only
                                std::nullopt, std::nullopt, std::nullopt,        // from_value, to_value, step_value
                                std::string("Allowed values: ") + kAllowedOutputStampModes); // additional_constraints
  // Allow user to optionally limit the per-cloud point count to a maximum
  this->declareAndLoadParameter("fixed_points_per_input_cloud", fixed_points_per_input_cloud_,
                                "Runtime-reconfigurable maximum valid point count per input cloud; 0 disables the limit",
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                kMinFixedPointsPerInputCloud,                    // from_value
                                kMaxFixedPointsPerInputCloud,                    // to_value
                                kStepSizeFixedPointsPerInputCloud,               // step_value
                                "0 = disabled; reasonable range is 0 to 10,000,000 points per input cloud");
  this->declareAndLoadParameter("use_cuda", use_cuda_,                           // name
                                "Runtime-reconfigurable backend selection; true uses CUDA and false uses CPU",
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                std::nullopt, std::nullopt, std::nullopt,        // from_value, to_value, step_value
                                "Runtime changes apply between fusion batches.");  // additional_constraints
  this->declareAndLoadParameter("range_limits.enable", range_limits_enable_,     // name
                                "Enable XYZ range filtering after transformation into target_frame",
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                std::nullopt, std::nullopt, std::nullopt,        // from_value, to_value, step_value
                                "When false, no range filtering is applied.");   // additional_constraints
  this->declareAndLoadParameter("range_limits.x_min", range_limits_x_min_,       // name
                                "Minimum x coordinate in target_frame to keep [m]",  // description
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                kMinRangeXY,                                     // from_value
                                kMaxRangeXY,                                     // to_value
                                std::nullopt,                                    // step_value
                                "Must be less than range_limits.x_max.");        // additional_constraints
  this->declareAndLoadParameter("range_limits.x_max", range_limits_x_max_,       // name
                                "Maximum x coordinate in target_frame to keep [m]",  // description
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                kMinRangeXY,                                     // from_value
                                kMaxRangeXY,                                     // to_value
                                std::nullopt,                                    // step_value
                                "Must be greater than range_limits.x_min.");     // additional_constraints
  this->declareAndLoadParameter("range_limits.y_min", range_limits_y_min_,       // name
                                "Minimum y coordinate in target_frame to keep [m]",  // description
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                kMinRangeXY,                                     // from_value
                                kMaxRangeXY,                                     // to_value
                                std::nullopt,                                    // step_value
                                "Must be less than range_limits.y_max.");        // additional_constraints
  this->declareAndLoadParameter("range_limits.y_max", range_limits_y_max_,       // name
                                "Maximum y coordinate in target_frame to keep [m]",  // description
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                kMinRangeXY,                                     // from_value
                                kMaxRangeXY,                                     // to_value
                                std::nullopt,                                    // step_value
                                "Must be greater than range_limits.y_min.");     // additional_constraints
  this->declareAndLoadParameter("range_limits.z_min", range_limits_z_min_,       // name
                                "Minimum z coordinate in target_frame to keep [m]",  // description
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                kMinRangeZ,                                      // from_value
                                kMaxRangeZ,                                      // to_value
                                std::nullopt,                                    // step_value
                                "Must be less than range_limits.z_max.");        // additional_constraints
  this->declareAndLoadParameter("range_limits.z_max", range_limits_z_max_,       // name
                                "Maximum z coordinate in target_frame to keep [m]",  // description
                                true,                                            // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                false,                                           // read_only
                                kMinRangeZ,                                      // from_value
                                kMaxRangeZ,                                      // to_value
                                std::nullopt,                                    // step_value
                                "Must be greater than range_limits.z_min.");     // additional_constraints
  validateRangeLimits();
  this->declareAndLoadParameter("max_time_diff_sec", max_time_diff_sec_,         // name
                                "Maximum timestamp spread across a synchronized input batch in seconds",
                                false,                                           // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                true,                                            // read_only
                                0.0, std::nullopt, std::nullopt,                 // from_value, to_value, step_value
                                "Must be non-negative");                         // additional_constraints
  this->declareAndLoadParameter("age_penalty", age_penalty_,                     // name
                                "Age penalty used by the approximate-time synchronizer",
                                false,                                           // add_to_auto_reconfigurable_params
                                false,                                           // is_required
                                true,                                            // read_only
                                0.0,                                             // from_value
                                100.0,                                           // to_value
                                std::nullopt,                                    // step_value
                                "Valid range is [0, 100].");                     // additional_constraints
  configureOutputStampMode(output_stamp_mode_param_);
 
#ifdef ENABLE_CUDA
  // Keep the CUDA context available even when starting in CPU mode so the
  // backend can be switched safely at runtime.
  try {
    cuda_context_ = std::make_unique<cuda::CudaTransformContext>();
    if (use_cuda_) {
      RCLCPP_INFO(this->get_logger(), "CUDA acceleration enabled");
    } else {
      RCLCPP_INFO(this->get_logger(), "CUDA context initialized; using CPU backend by parameter");
    }
  } catch (const std::exception& e) {
    RCLCPP_ERROR(this->get_logger(), "Failed to initialize CUDA context: %s", e.what());
    RCLCPP_WARN(this->get_logger(), "Falling back to CPU-only implementation");
    cuda_context_.reset();
    use_cuda_ = false;
    this->set_parameter(rclcpp::Parameter("use_cuda", false));
  }
#else
  RCLCPP_INFO(this->get_logger(), "CUDA support not compiled, using CPU-only implementation");
  use_cuda_ = false;
  this->set_parameter(rclcpp::Parameter("use_cuda", false));
#endif
 
  // run setup after constructor has finished to enable shared_from_this()
  setup_timer_ = this->create_wall_timer(std::chrono::milliseconds(1), [this]() {
    setup();
    setup_timer_->cancel();
  });
}

Member Function Documentation

collectTimingInfo()

bool point_cloud_fusion::PointCloudFusion::collectTimingInfo ( const std::vector< PointCloudMsg::ConstSharedPtr > & msgs, FusionTiming & timing ) const

private

Collect timestamp statistics for synchronized input clouds.

Parameters

msgs	Synchronized input point clouds.
timing	Output timing metadata populated from the batch.

Returns

True if timing metadata could be collected for the batch.

Definition at line 829 of file point_cloud_fusion.cpp.

                                                                                                                       {
  if (msgs.empty()) {
    return false;
  }
 
  const auto input0_stamp = rclcpp::Time(msgs.front()->header.stamp);
  bool first_stamp = true;
  double max_dt_from_input0_sec = 0.0;
  rclcpp::Time earliest_stamp;
  rclcpp::Time latest_stamp;
 
  // Walk every cloud once to gather min/max stamps and the largest skew from
  // input0.
  for (const auto& pc_msg : msgs) {
    if (!pc_msg) {
      RCLCPP_WARN(this->get_logger(),
                  "Received null point cloud pointer in "
                  "synchronized batch, skipping fusion");
      return false;
    }
 
    const rclcpp::Time current_stamp(pc_msg->header.stamp);
    if (first_stamp) {
      earliest_stamp = current_stamp;
      latest_stamp = current_stamp;
      first_stamp = false;
    } else {
      if (current_stamp < earliest_stamp) earliest_stamp = current_stamp;
      if (current_stamp > latest_stamp) latest_stamp = current_stamp;
    }
 
    const double dt_sec = std::fabs((current_stamp - input0_stamp).seconds());
    if (dt_sec > max_dt_from_input0_sec) {
      max_dt_from_input0_sec = dt_sec;
    }
  }
 
  timing.earliest_stamp = earliest_stamp;
  timing.latest_stamp = latest_stamp;
  timing.input0_stamp = input0_stamp;
  timing.max_dt_from_input0_sec = max_dt_from_input0_sec;
  return true;
}

configureOutputStampMode()

void point_cloud_fusion::PointCloudFusion::configureOutputStampMode ( const std::string & mode )

private

Parse and store the configured output timestamp mode.

Parameters

mode	Timestamp mode parameter value.

Definition at line 1232 of file point_cloud_fusion.cpp.

                                                                     {
  std::string lowered = mode;
  std::transform(lowered.begin(), lowered.end(), lowered.begin(),
                 [](unsigned char c) { return static_cast<char>(std::tolower(c)); });
 
  if (lowered == "earliest") {
    output_stamp_mode_ = OutputStampMode::Earliest;
  } else if (lowered == "mean" || lowered == "mid" || lowered == "midpoint") {
    output_stamp_mode_ = OutputStampMode::Mean;
  } else if (lowered == "input0") {
    output_stamp_mode_ = OutputStampMode::Input0;
  } else if (lowered == "latest") {
    output_stamp_mode_ = OutputStampMode::Latest;
  } else {
    RCLCPP_WARN(this->get_logger(), "Invalid output_stamp_mode '%s'; defaulting to 'earliest'.", mode.c_str());
    output_stamp_mode_ = OutputStampMode::Earliest;
  }
}

declareAndLoadParameter()

template<typename T >

void point_cloud_fusion::PointCloudFusion::declareAndLoadParameter ( const std::string & name, T & param, const std::string & description, const bool add_to_auto_reconfigurable_params = true, const bool is_required = false, const bool read_only = false, const std::optional< double > & from_value = std::nullopt, const std::optional< double > & to_value = std::nullopt, const std::optional< double > & step_value = std::nullopt, const std::string & additional_constraints = "" )

private

Declares and loads a ROS parameter.

Parameters

name	name
param	parameter variable to load into
description	description
add_to_auto_reconfigurable_params	enable reconfiguration of parameter
is_required	whether failure to load parameter will stop node
read_only	set parameter to read-only
from_value	parameter range minimum
to_value	parameter range maximum
step_value	parameter range step
additional_constraints	additional constraints description

Definition at line 300 of file point_cloud_fusion.cpp.

                                                                                        {
  rcl_interfaces::msg::ParameterDescriptor param_desc;
  param_desc.description = description;
  param_desc.additional_constraints = additional_constraints;
  param_desc.read_only = read_only;
 
  auto type = rclcpp::ParameterValue(param).get_type();
 
  if (from_value.has_value() && to_value.has_value()) {
    if constexpr (std::is_integral_v<T>) {
      rcl_interfaces::msg::IntegerRange range;
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
      if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
      param_desc.integer_range = {range};
    } else if constexpr (std::is_floating_point_v<T>) {
      rcl_interfaces::msg::FloatingPointRange range;
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
      if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
      param_desc.floating_point_range = {range};
    } else {
      RCLCPP_WARN(this->get_logger(),
                  "Parameter type of parameter '%s' does not support "
                  "specifying a range",
                  name.c_str());
    }
  }
 
  this->declare_parameter(name, type, param_desc);
 
  try {
    param = this->get_parameter(name).get_value<T>();
    std::stringstream ss;
    ss << "Loaded parameter '" << name << "': ";
    if constexpr (is_vector_v<T>) {
      ss << "[";
      for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
      ss << "]";
    } else {
      ss << param;
    }
    RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
  } catch (rclcpp::exceptions::ParameterUninitializedException&) {
    if (is_required) {
      RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting");
      exit(EXIT_FAILURE);
    } else {
      std::stringstream ss;
      ss << "Missing parameter '" << name << "', using default value: ";
      if constexpr (is_vector_v<T>) {
        ss << "[";
        for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
        ss << "]";
      } else {
        ss << param;
      }
      RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
      this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
    }
  }
 
  if (add_to_auto_reconfigurable_params) {
    std::function<void(const rclcpp::Parameter&)> setter = [&param](const rclcpp::Parameter& p) { param = p.get_value<T>(); };
    auto_reconfigurable_params_.push_back(std::make_tuple(name, setter));
  }
}

fusePointCloudBatch()

PointCloudFusion::PointCloudMsg::UniquePtr point_cloud_fusion::PointCloudFusion::fusePointCloudBatch ( const std::vector< PointCloudMsg::ConstSharedPtr > & msgs, const FusionTiming & timing, std::size_t & valid_point_count ) const

private

Fuse a synchronized point-cloud batch using the CPU path.

Parameters

msgs	Synchronized input point clouds.
timing	Timing metadata for the batch.
valid_point_count	Number of valid points written to the output cloud.

Returns

Fused point cloud.

Definition at line 873 of file point_cloud_fusion.cpp.

                                                                                                                        {
  if (msgs.empty()) {
    return nullptr;
  }
 
  const auto& input0_msg = msgs.front();
  if (!input0_msg) {
    return nullptr;
  }
 
  std::optional<uint32_t> x_offset;
  std::optional<uint32_t> y_offset;
  std::optional<uint32_t> z_offset;
  for (const auto& field : input0_msg->fields) {
    if (field.name == "x") {
      x_offset = field.offset;
    } else if (field.name == "y") {
      y_offset = field.offset;
    } else if (field.name == "z") {
      z_offset = field.offset;
    }
  }
 
  if (!x_offset || !y_offset || !z_offset) {
    RCLCPP_WARN(this->get_logger(), "Point cloud lacks x/y/z fields; skipping fusion for this batch.");
    valid_point_count = 0;
    return nullptr;
  }
 
  const size_t point_step = input0_msg->point_step;
  const auto& input0_fields = input0_msg->fields;
  const bool is_bigendian = input0_msg->is_bigendian;
 
  struct FieldCopyPlan {
    const sensor_msgs::msg::PointField* source;
    sensor_msgs::msg::PointField destination;
    std::size_t byte_length;
  };
 
  bool use_all_fields = output_fields_.empty();
  std::vector<FieldCopyPlan> copy_plan;
  std::vector<sensor_msgs::msg::PointField> fused_fields;
  fused_fields.reserve(input0_fields.size());
 
  std::size_t fused_point_step = point_step;
  std::optional<uint32_t> fused_x_offset = x_offset;
  std::optional<uint32_t> fused_y_offset = y_offset;
  std::optional<uint32_t> fused_z_offset = z_offset;
 
  if (!use_all_fields) {
    fused_x_offset.reset();
    fused_y_offset.reset();
    fused_z_offset.reset();
    bool selection_valid = true;
    fused_point_step = 0;
    fused_fields.clear();
    copy_plan.reserve(output_fields_.size());
 
    for (const auto& requested_name : output_fields_) {
      auto iter =
          std::find_if(input0_fields.begin(), input0_fields.end(),
                       [&requested_name](const sensor_msgs::msg::PointField& field) { return field.name == requested_name; });
      if (iter == input0_fields.end()) {
        RCLCPP_WARN(this->get_logger(),
                    "Requested output field '%s' not present in incoming point "
                    "cloud; publishing full field set instead.",
                    requested_name.c_str());
        selection_valid = false;
        break;
      }
 
      const std::size_t datatype_size = pointFieldDatatypeSize(iter->datatype);
      if (datatype_size == 0) {
        RCLCPP_WARN(this->get_logger(),
                    "Point field '%s' uses unsupported datatype %u; publishing "
                    "full field set instead.",
                    requested_name.c_str(), static_cast<unsigned int>(iter->datatype));
        selection_valid = false;
        break;
      }
 
      FieldCopyPlan plan;
      plan.source = &(*iter);
      plan.destination = *iter;
      plan.destination.offset = static_cast<uint32_t>(fused_point_step);
      plan.byte_length = datatype_size * static_cast<std::size_t>(iter->count);
      fused_point_step += plan.byte_length;
 
      if (requested_name == "x") {
        fused_x_offset = plan.destination.offset;
      } else if (requested_name == "y") {
        fused_y_offset = plan.destination.offset;
      } else if (requested_name == "z") {
        fused_z_offset = plan.destination.offset;
      }
 
      copy_plan.push_back(plan);
      fused_fields.push_back(plan.destination);
    }
 
    if (!selection_valid || !fused_x_offset || !fused_y_offset || !fused_z_offset) {
      if (selection_valid) {
        RCLCPP_ERROR(this->get_logger(),
                     "Output field selection must include x, y, and z; "
                     "publishing full field set instead.");
      }
      use_all_fields = true;
      copy_plan.clear();
      fused_point_step = point_step;
      fused_fields.assign(input0_fields.begin(), input0_fields.end());
      fused_x_offset = x_offset;
      fused_y_offset = y_offset;
      fused_z_offset = z_offset;
    }
  }
 
  if (use_all_fields) {
    fused_fields.assign(input0_fields.begin(), input0_fields.end());
    fused_point_step = point_step;
    fused_x_offset = x_offset;
    fused_y_offset = y_offset;
    fused_z_offset = z_offset;
  }
 
  // Reserve enough space once so the fusion loop only appends into a pre-sized
  // buffer.
  const size_t max_capacity = std::accumulate(
      msgs.begin(), msgs.end(), static_cast<size_t>(0), [this](size_t sum, const PointCloudMsg::ConstSharedPtr& cloud) {
        if (!cloud) {
          return sum;
        }
        size_t cloud_size = static_cast<size_t>(cloud->width) * static_cast<size_t>(cloud->height);
        // If user set a cap, limit each cloud to that size
        if (fixed_points_per_input_cloud_ > 0) {
          cloud_size = std::min(cloud_size, static_cast<size_t>(fixed_points_per_input_cloud_));
        }
        return sum + cloud_size;
      });
 
  if (max_capacity == 0) {
    valid_point_count = 0;
    return nullptr;
  }
 
  auto output = std::make_unique<PointCloudMsg>();
  output->header.frame_id = target_frame_;
  rclcpp::Time chosen_stamp;
  switch (output_stamp_mode_) {
    case OutputStampMode::Earliest:
      chosen_stamp = timing.earliest_stamp;
      break;
    case OutputStampMode::Mean: {
      const auto delta = timing.latest_stamp - timing.earliest_stamp;
      chosen_stamp = timing.earliest_stamp + rclcpp::Duration::from_nanoseconds(delta.nanoseconds() / 2);
      break;
    }
    case OutputStampMode::Input0:
      chosen_stamp = timing.input0_stamp;
      break;
    case OutputStampMode::Latest:
    default:
      chosen_stamp = timing.latest_stamp;
      break;
  }
  output->header.stamp = chosen_stamp;
  output->height = 1;
  output->is_bigendian = is_bigendian;
  output->point_step = fused_point_step;
  output->fields = fused_fields;
  output->is_dense = true;
  output->data.resize(max_capacity * fused_point_step);
 
  uint8_t* dest_ptr = output->data.data();
  valid_point_count = 0;
  std::size_t skipped_inputs = 0;
 
  // Pre-cache range limits as float to avoid per-point double→float conversion.
  const bool check_range = range_limits_enable_;
  const float rl_x_min = static_cast<float>(range_limits_x_min_);
  const float rl_x_max = static_cast<float>(range_limits_x_max_);
  const float rl_y_min = static_cast<float>(range_limits_y_min_);
  const float rl_y_max = static_cast<float>(range_limits_y_max_);
  const float rl_z_min = static_cast<float>(range_limits_z_min_);
  const float rl_z_max = static_cast<float>(range_limits_z_max_);
 
  for (const auto& msg : msgs) {
    if (!msg) {
      continue;
    }
 
    if (msg->point_step != point_step || msg->fields != input0_fields) {
      RCLCPP_WARN(this->get_logger(), "Skipping point cloud '%s' due to incompatible field layout.",
                  msg->header.frame_id.c_str());
      ++skipped_inputs;
      continue;
    }
 
    // Cache the frame transform once per cloud to avoid repeated TF queries
    // inside the point loop.
    const bool apply_transform = msg->header.frame_id != target_frame_;
    tf2::Vector3 translation;
    tf2::Matrix3x3 rotation;
    if (apply_transform) {
      tf2::Transform tf_transform;
      geometry_msgs::msg::TransformStamped tf_stamped;
      try {
        tf_stamped = tf_buffer_->lookupTransform(target_frame_, msg->header.frame_id, msg->header.stamp,
                                                 rclcpp::Duration::from_seconds(0.1));
      } catch (const tf2::TransformException& ex) {
        RCLCPP_ERROR(this->get_logger(), "Cannot transform point cloud from %s to %s: %s", msg->header.frame_id.c_str(),
                     target_frame_.c_str(), ex.what());
        ++skipped_inputs;
        continue;
      }
      tf2::fromMsg(tf_stamped.transform, tf_transform);
      translation = tf_transform.getOrigin();
      rotation = tf_transform.getBasis();
    }
 
    const auto* src_data = msg->data.data();
    const size_t total_points = static_cast<size_t>(msg->width) * static_cast<size_t>(msg->height);
 
    auto emit_point = [&](const uint8_t* point_ptr, float x, float y, float z, bool overwrite_xyz) {
      if (use_all_fields) {
        std::memcpy(dest_ptr, point_ptr, point_step);
      } else {
        for (const auto& plan : copy_plan) {
          std::memcpy(byteOffset(dest_ptr, plan.destination.offset), byteOffset(point_ptr, plan.source->offset),
                      plan.byte_length);
        }
      }
 
      if (overwrite_xyz) {
        storeFloat(dest_ptr, *fused_x_offset, x);
        storeFloat(dest_ptr, *fused_y_offset, y);
        storeFloat(dest_ptr, *fused_z_offset, z);
      }
 
      dest_ptr = byteOffset(dest_ptr, fused_point_step);
      ++valid_point_count;
    };
 
    if (fixed_points_per_input_cloud_ <= 0 || static_cast<size_t>(fixed_points_per_input_cloud_) >= total_points) {
      // Fast path: when no downsampling is requested.
      for (size_t idx = 0; idx < total_points; ++idx) {
        const auto* point_ptr = byteOffset(src_data, idx * point_step);
        const float x = loadFloat(point_ptr, *x_offset);
        const float y = loadFloat(point_ptr, *y_offset);
        const float z = loadFloat(point_ptr, *z_offset);
 
        if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) {
          continue;
        }
 
        if (!apply_transform) {
          if (check_range && !pointWithinRange(x, y, z, rl_x_min, rl_x_max, rl_y_min, rl_y_max, rl_z_min, rl_z_max)) {
            continue;
          }
          emit_point(point_ptr, x, y, z, false);
          continue;
        }
 
        const tf2::Vector3 rotated = rotation * tf2::Vector3(x, y, z) + translation;
        const float transformed_x = static_cast<float>(rotated.x());
        const float transformed_y = static_cast<float>(rotated.y());
        const float transformed_z = static_cast<float>(rotated.z());
 
        if (check_range && !pointWithinRange(transformed_x, transformed_y, transformed_z, rl_x_min, rl_x_max, rl_y_min, rl_y_max,
                                             rl_z_min, rl_z_max)) {
          continue;
        }
 
        emit_point(point_ptr, transformed_x, transformed_y, transformed_z, true);
      }
      continue;
    }
 
    // Downsample path: strided sampling for uniform spatial distribution.
    const size_t desired_points = static_cast<size_t>(fixed_points_per_input_cloud_);
    const double stride = static_cast<double>(total_points) / static_cast<double>(desired_points);
    const size_t num_samples = desired_points;
 
    for (size_t i = 0; i < num_samples; ++i) {
      const size_t idx = std::min(static_cast<size_t>(static_cast<double>(i) * stride), total_points - 1);
 
      const auto* point_ptr = byteOffset(src_data, idx * point_step);
      const float x = loadFloat(point_ptr, *x_offset);
      const float y = loadFloat(point_ptr, *y_offset);
      const float z = loadFloat(point_ptr, *z_offset);
 
      if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) {
        continue;
      }
 
      if (!apply_transform) {
        if (check_range && !pointWithinRange(x, y, z, rl_x_min, rl_x_max, rl_y_min, rl_y_max, rl_z_min, rl_z_max)) {
          continue;
        }
        emit_point(point_ptr, x, y, z, false);
        continue;
      }
 
      const tf2::Vector3 rotated = rotation * tf2::Vector3(x, y, z) + translation;
      const float transformed_x = static_cast<float>(rotated.x());
      const float transformed_y = static_cast<float>(rotated.y());
      const float transformed_z = static_cast<float>(rotated.z());
 
      if (check_range && !pointWithinRange(transformed_x, transformed_y, transformed_z, rl_x_min, rl_x_max, rl_y_min, rl_y_max,
                                           rl_z_min, rl_z_max)) {
        continue;
      }
 
      emit_point(point_ptr, transformed_x, transformed_y, transformed_z, true);
    }
  }
 
  if (valid_point_count == 0) {
    if (skipped_inputs == msgs.size()) {
      RCLCPP_WARN(this->get_logger(), "Skipped all point clouds in synchronized batch; no data fused.");
    }
    return nullptr;
  }
 
  output->width = valid_point_count;
  output->row_step = output->point_step * output->width;
  output->data.resize(valid_point_count * fused_point_step);
 
  output->is_dense = true;
  return output;
}

handleSynchronizedPointClouds()

void point_cloud_fusion::PointCloudFusion::handleSynchronizedPointClouds ( const std::vector< sensor_msgs::msg::PointCloud2::ConstSharedPtr > & msgs )

private

Process synchronized point clouds.

Parameters

msgs	batch of synchronized point clouds

Definition at line 768 of file point_cloud_fusion.cpp.

                                                                                                                   {
  if (msgs.empty()) {
    return;
  }
 
  // Protect runtime-configurable parameter reads against concurrent parameter
  // updates.
  std::shared_lock<std::shared_mutex> config_lock(config_mutex_);
 
  const auto callback_start = std::chrono::steady_clock::now();
 
  FusionTiming timing;
  if (!collectTimingInfo(msgs, timing)) {
    return;
  }
 
#ifdef ENABLE_CUDA
  std::size_t cuda_valid_count = 0;
  auto processing_start = std::chrono::steady_clock::time_point{};
  auto processing_end = std::chrono::steady_clock::time_point{};
  PointCloudMsg::UniquePtr cuda_result;
  bool used_cuda = false;
  {
    // Guard shared CUDA pipeline state against concurrent synchronized
    // callbacks.
    std::lock_guard<std::mutex> cuda_lock(cuda_context_mutex_);
    // Run either CPU or CUDA implementation based on parameter.
    if (cuda_context_ && use_cuda_) {
      used_cuda = true;
      processing_start = std::chrono::steady_clock::now();
      cuda_result = fusePointCloudBatchCUDA(msgs, timing, cuda_valid_count);
      processing_end = std::chrono::steady_clock::now();
    }
  }
 
  if (used_cuda) {
    if (cuda_result) {
      publishFusedCloud(std::move(cuda_result), timing, msgs.size(), cuda_valid_count, callback_start, processing_start,
                        processing_end, "cuda_fusion_complete");
    } else {
      RCLCPP_WARN(this->get_logger(), "CUDA processing failed");
    }
    return;
  }
#endif
 
  // CPU-only path
  std::size_t valid_count = 0;
  const auto cpu_processing_start = std::chrono::steady_clock::now();
  auto fused_point_cloud = fusePointCloudBatch(msgs, timing, valid_count);
 
  if (!fused_point_cloud) {
    RCLCPP_WARN(this->get_logger(), "All points are invalid, skipping fusion");
    return;
  }
 
  const auto cpu_processing_end = std::chrono::steady_clock::now();
  publishFusedCloud(std::move(fused_point_cloud), timing, msgs.size(), valid_count, callback_start, cpu_processing_start,
                    cpu_processing_end, "cpu_fusion_complete");
}

parametersCallback()

rcl_interfaces::msg::SetParametersResult point_cloud_fusion::PointCloudFusion::parametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

private

Validate and apply dynamic parameter updates.

Parameters

parameters

ROS parameters requested for update.

Returns

Result indicating whether the update was accepted.

Definition at line 375 of file point_cloud_fusion.cpp.

                                                                                                                      {
  std::unique_lock<std::shared_mutex> config_lock(config_mutex_);
 
  // Pre-validate interdependent and runtime-sensitive parameters before
  // applying any changes.
  // Build the prospective state: current values overridden by incoming changes.
  bool any_range_param = false;
  bool prospective_use_cuda = use_cuda_;
  int64_t prospective_fixed_points_per_input_cloud = fixed_points_per_input_cloud_;
  double prospective_x_min = range_limits_x_min_;
  double prospective_x_max = range_limits_x_max_;
  double prospective_y_min = range_limits_y_min_;
  double prospective_y_max = range_limits_y_max_;
  double prospective_z_min = range_limits_z_min_;
  double prospective_z_max = range_limits_z_max_;
 
  for (const auto& param : parameters) {
    const auto& name = param.get_name();
    if (name == "fixed_points_per_input_cloud") {
      prospective_fixed_points_per_input_cloud = param.as_int();
    } else if (name == "use_cuda") {
      prospective_use_cuda = param.as_bool();
    } else if (name == "range_limits.x_min") {
      prospective_x_min = param.as_double();
      any_range_param = true;
    } else if (name == "range_limits.x_max") {
      prospective_x_max = param.as_double();
      any_range_param = true;
    } else if (name == "range_limits.y_min") {
      prospective_y_min = param.as_double();
      any_range_param = true;
    } else if (name == "range_limits.y_max") {
      prospective_y_max = param.as_double();
      any_range_param = true;
    } else if (name == "range_limits.z_min") {
      prospective_z_min = param.as_double();
      any_range_param = true;
    } else if (name == "range_limits.z_max") {
      prospective_z_max = param.as_double();
      any_range_param = true;
    }
  }
 
  if (prospective_fixed_points_per_input_cloud < kMinFixedPointsPerInputCloud ||
      prospective_fixed_points_per_input_cloud > kMaxFixedPointsPerInputCloud) {
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = false;
    result.reason = "fixed_points_per_input_cloud must be in [" + std::to_string(kMinFixedPointsPerInputCloud) + ", " +
                    std::to_string(kMaxFixedPointsPerInputCloud) + "]";
    RCLCPP_ERROR(this->get_logger(), "Rejecting parameter update: %s", result.reason.c_str());
    return result;
  }
 
  if (prospective_use_cuda) {
#ifdef ENABLE_CUDA
    if (!cuda_context_) {
      rcl_interfaces::msg::SetParametersResult result;
      result.successful = false;
      result.reason =
          "CUDA backend is unavailable because CUDA context "
          "initialization failed";
      RCLCPP_ERROR(this->get_logger(), "Rejecting parameter update: %s", result.reason.c_str());
      return result;
    }
#else
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = false;
    result.reason = "CUDA backend is unavailable because CUDA support was not compiled";
    RCLCPP_ERROR(this->get_logger(), "Rejecting parameter update: %s", result.reason.c_str());
    return result;
#endif
  }
 
  if (any_range_param) {
    rcl_interfaces::msg::SetParametersResult result;
    std::string reason;
    if (prospective_x_min >= prospective_x_max) {
      reason += "range_limits.x_min (" + std::to_string(prospective_x_min) + ") must be less than range_limits.x_max (" +
                std::to_string(prospective_x_max) + "). ";
    }
    if (prospective_y_min >= prospective_y_max) {
      reason += "range_limits.y_min (" + std::to_string(prospective_y_min) + ") must be less than range_limits.y_max (" +
                std::to_string(prospective_y_max) + "). ";
    }
    if (prospective_z_min >= prospective_z_max) {
      reason += "range_limits.z_min (" + std::to_string(prospective_z_min) + ") must be less than range_limits.z_max (" +
                std::to_string(prospective_z_max) + "). ";
    }
    if (!reason.empty()) {
      result.successful = false;
      result.reason = reason;
      RCLCPP_ERROR(this->get_logger(), "Rejecting range_limits parameter update: %s", reason.c_str());
      return result;
    }
  }
 
  // All validations passed — apply changes.
  const bool previous_use_cuda = use_cuda_;
  for (const auto& param : parameters) {
    for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) {
      if (param.get_name() == std::get<0>(auto_reconfigurable_param)) {
        std::get<1>(auto_reconfigurable_param)(param);
        RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s' to: %s", param.get_name().c_str(),
                    param.value_to_string().c_str());
        break;
      }
    }
  }
  if (use_cuda_ != previous_use_cuda) {
    RCLCPP_INFO(this->get_logger(), "Fusion backend switched to %s", use_cuda_ ? "CUDA" : "CPU");
  }
 
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
 
  return result;
}

publishFusedCloud()

void point_cloud_fusion::PointCloudFusion::publishFusedCloud ( PointCloudMsg::UniquePtr cloud, const FusionTiming & timing, std::size_t input_count, std::size_t total_points, std::chrono::steady_clock::time_point callback_start, std::chrono::steady_clock::time_point processing_start, std::chrono::steady_clock::time_point processing_end, const char * event_name )

private

Publish a fused point cloud and emit tracing/timing diagnostics.

Parameters

cloud	Fused point cloud to publish.
timing	Timing metadata for the synchronized input batch.
input_count	Number of input clouds in the batch.
total_points	Number of points before filtering.
callback_start	Time when the synchronized callback started.
processing_start	Time when fusion processing started.
processing_end	Time when fusion processing ended.
event_name	Trace event name for the selected backend.

Definition at line 1205 of file point_cloud_fusion.cpp.

                                                                 {
  // Publish the fused cloud and emit a compact timing summary for
  // observability.
  cloud_publisher_->publish(std::move(cloud));
  const auto publish_end = std::chrono::steady_clock::now();
 
  const double prep_duration_ms = std::chrono::duration<double, std::milli>(processing_start - callback_start).count();
  const double processing_duration_ms = std::chrono::duration<double, std::milli>(processing_end - processing_start).count();
  const double publish_duration_ms = std::chrono::duration<double, std::milli>(publish_end - processing_end).count();
  const double e2e_duration_ms = prep_duration_ms + processing_duration_ms + publish_duration_ms;
  const double batch_dt_ms = (timing.latest_stamp - timing.earliest_stamp).seconds() * 1000.0;
 
  RCLCPP_DEBUG(this->get_logger(),
               "%s inputs=%zu points=%zu e2e_ms=%.3f prep_ms=%.3f "
               "process_ms=%.3f publish_ms=%.3f batch_dt_ms=%.3f "
               "max_dt_ms=%.3f",
               event_name, input_count, total_points, e2e_duration_ms, prep_duration_ms, processing_duration_ms,
               publish_duration_ms, batch_dt_ms, timing.max_dt_from_input0_sec * 1000.0);
}

setup()

void point_cloud_fusion::PointCloudFusion::setup ( )

private

Sets up subscribers, publishers, etc. to configure the node.

Definition at line 609 of file point_cloud_fusion.cpp.

                             {
  // callback for dynamic parameter configuration
  parameters_callback_ =
      this->add_on_set_parameters_callback(std::bind(&PointCloudFusion::parametersCallback, this, std::placeholders::_1));
 
  // create transform buffer and listener
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
  // validate inputs
  if (!input_transport_hints_.empty() && input_transport_hints_.size() != input_topics_.size()) {
    RCLCPP_WARN(this->get_logger(),
                "'input_transport_hints' length (%zu) does not match "
                "'input_topics' (%zu). Missing hints default to "
                "'%s'",
                input_transport_hints_.size(), input_topics_.size(), kDefaultTransportHint);
  }
 
  // create subscribers
  cloud_subscribers_.clear();
  cloud_subscriber_callback_groups_.clear();
  cloud_subscribers_.reserve(input_topics_.size());
  cloud_subscriber_callback_groups_.reserve(input_topics_.size());
  for (size_t i = 0; i < input_topics_.size(); ++i) {
    const std::string configured = input_topics_[i];
    const std::string resolved = this->get_node_topics_interface()->resolve_topic_name(configured);
    const std::string hint = (i < input_transport_hints_.size() && !input_transport_hints_[i].empty())
                                 ? input_transport_hints_[i]
                                 : std::string(kDefaultTransportHint);
 
    auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    rclcpp::SubscriptionOptions subscription_options;
    subscription_options.callback_group = callback_group;
 
    auto subscriber = std::make_shared<point_cloud_transport::SubscriberFilter>();
    subscriber->subscribe(this->shared_from_this(), resolved, hint, rmw_qos_profile_default, subscription_options);
    RCLCPP_INFO(this->get_logger(), "Subscribed to '%s' (hint=%s)", subscriber->getTopic().c_str(), hint.c_str());
    cloud_subscriber_callback_groups_.push_back(std::move(callback_group));
    cloud_subscribers_.push_back(std::move(subscriber));
  }
 
  RCLCPP_INFO(this->get_logger(), "Configured %zu input subscriber callback groups for %zu input topics",
              cloud_subscriber_callback_groups_.size(), input_topics_.size());
 
  synchronizer_.reset();
 
  // configure synchronization or direct passthrough
  if (cloud_subscribers_.size() == 1) {
    cloud_subscribers_.front()->registerCallback([this](const PointCloudMsg::ConstSharedPtr msg) {
      std::vector<PointCloudMsg::ConstSharedPtr> batch;
      batch.reserve(1);
      batch.emplace_back(msg);
      this->handleSynchronizedPointClouds(batch);
    });
    RCLCPP_INFO(this->get_logger(), "Configured single-input mode for topic '%s'",
                cloud_subscribers_.front()->getTopic().c_str());
  } else if (cloud_subscribers_.size() <= 9) {
    switch (cloud_subscribers_.size()) {
      case 2:
        setupSynchronizer<2>();
        break;
      case 3:
        setupSynchronizer<3>();
        break;
      case 4:
        setupSynchronizer<4>();
        break;
      case 5:
        setupSynchronizer<5>();
        break;
      case 6:
        setupSynchronizer<6>();
        break;
      case 7:
        setupSynchronizer<7>();
        break;
      case 8:
        setupSynchronizer<8>();
        break;
      case 9:
        setupSynchronizer<9>();
        break;
      default:
        RCLCPP_FATAL(this->get_logger(), "Unsupported number of input topics: %zu", cloud_subscribers_.size());
        exit(EXIT_FAILURE);
    }
  }
 
  // create publisher
  point_cloud_transport::PointCloudTransport pct(this->shared_from_this());
  std::string point_cloud_topic_name = this->get_node_topics_interface()->resolve_topic_name("~/point_cloud");
  cloud_publisher_ = std::make_shared<point_cloud_transport::Publisher>(
      pct.advertise(point_cloud_topic_name, static_cast<uint32_t>(output_queue_size_)));
  RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", cloud_publisher_->getTopic().c_str());
 
  // Annotate message links for tracing: Each publisher (for raw and compressed
  // point clouds) depends an all input point clouds.
  std::vector<const void*> link_subs;
  std::vector<const void*> link_pubs;
  for (const auto& sub_filter : cloud_subscribers_) {
    auto sub_base = sub_filter->getSubscriber().getSubscription();
    if (sub_base) {
      link_subs.push_back(static_cast<const void*>(sub_base->get_subscription_handle().get()));
    }
  }
  if (cloud_publisher_) {
    std::map<std::string, rclcpp::PublisherBase::SharedPtr> pubs_base = cloud_publisher_->getPublishers();
    for (const auto& [transport, pub_base] : pubs_base) {
      link_pubs.push_back(static_cast<const void*>(pub_base->get_publisher_handle().get()));
    }
  }
  TRACETOOLS_TRACEPOINT(message_link_partial_sync, link_subs.data(), link_subs.size(), link_pubs.data(), link_pubs.size());
}

setupSynchronizer()

template<std::size_t N>

void point_cloud_fusion::PointCloudFusion::setupSynchronizer ( )

private

Create the approximate-time synchronizer for a fixed number of input topics.

Template Parameters

N	Number of synchronized point-cloud inputs.

Definition at line 724 of file point_cloud_fusion.cpp.

                                         {
  static_assert(N >= 2 && N <= 9, "Supported synchronizer size is between 2 and 9");
 
  using Policy = detail::SyncPolicy<N>;
  using Sync = detail::SyncType<N>;
 
  // Instantiate ApproximateTime policy tuned to the active input count.
  auto sync = std::make_shared<Sync>(Policy(static_cast<size_t>(sync_queue_size_)));
 
  // Wire the configured subscribers into the synchronizer slots.
  detail::connectInputs<N>(*sync, cloud_subscribers_);
 
  sync->setMaxIntervalDuration(rclcpp::Duration::from_seconds(max_time_diff_sec_));
  sync->setAgePenalty(age_penalty_);
  sync->registerCallback([this](auto&&... msgs) {
    std::vector<PointCloudMsg::ConstSharedPtr> batch;
    batch.reserve(sizeof...(msgs));
    // Extract only valid PointCloud2 pointers from the variadic callback.
    auto append = [&batch](auto&& msg) {
      using ArgT = std::decay_t<decltype(msg)>;
      if constexpr (std::is_same_v<ArgT, PointCloudMsg::ConstSharedPtr>) {
        if (msg) {
          batch.emplace_back(std::forward<decltype(msg)>(msg));
        }
      }
    };
    (append(std::forward<decltype(msgs)>(msgs)), ...);
    if (!batch.empty()) {
      this->handleSynchronizedPointClouds(batch);
    } else {
      RCLCPP_WARN(this->get_logger(),
                  "ApproximateTime synchronizer yielded no "
                  "valid point clouds; skipping fusion.");
    }
  });
 
  synchronizer_ = sync;
 
  RCLCPP_INFO(this->get_logger(),
              "Configured approximate time synchronizer for %zu inputs "
              "(queue=%zu, max_dt=%.3f s, age_penalty=%.6f)",
              static_cast<size_t>(N), static_cast<size_t>(sync_queue_size_), max_time_diff_sec_, age_penalty_);
}

validateInputTopicsParameter()

void point_cloud_fusion::PointCloudFusion::validateInputTopicsParameter ( ) const

private

Validate that the configured input topic list is usable.

Definition at line 1251 of file point_cloud_fusion.cpp.

                                                          {
  if (input_topics_.empty()) {
    RCLCPP_FATAL(this->get_logger(), "No input topics configured (parameter 'input_topics'). Exiting");
    exit(EXIT_FAILURE);
  }
  if (input_topics_.size() > kMaxInputTopics) {
    RCLCPP_FATAL(this->get_logger(),
                 "Configured with %zu input topics, but only up to %zu inputs "
                 "are supported",
                 input_topics_.size(), kMaxInputTopics);
    exit(EXIT_FAILURE);
  }
}

validateRangeLimits()

void point_cloud_fusion::PointCloudFusion::validateRangeLimits ( )

private

Validate configured XYZ range limits and disable filtering when invalid.

Definition at line 1265 of file point_cloud_fusion.cpp.

                                           {
  bool valid = true;
  if (range_limits_x_min_ >= range_limits_x_max_) {
    RCLCPP_ERROR(this->get_logger(),
                 "range_limits.x_min (%.3f) must be less than "
                 "range_limits.x_max (%.3f); disabling range filtering",
                 range_limits_x_min_, range_limits_x_max_);
    valid = false;
  }
  if (range_limits_y_min_ >= range_limits_y_max_) {
    RCLCPP_ERROR(this->get_logger(),
                 "range_limits.y_min (%.3f) must be less than "
                 "range_limits.y_max (%.3f); disabling range filtering",
                 range_limits_y_min_, range_limits_y_max_);
    valid = false;
  }
  if (range_limits_z_min_ >= range_limits_z_max_) {
    RCLCPP_ERROR(this->get_logger(),
                 "range_limits.z_min (%.3f) must be less than "
                 "range_limits.z_max (%.3f); disabling range filtering",
                 range_limits_z_min_, range_limits_z_max_);
    valid = false;
  }
  if (!valid) {
    range_limits_enable_ = false;
  }
}

Member Data Documentation

age_penalty_

double point_cloud_fusion::PointCloudFusion::age_penalty_ = 0.1

private

Definition at line 216 of file point_cloud_fusion.hpp.

auto_reconfigurable_params_

std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)> > > point_cloud_fusion::PointCloudFusion::auto_reconfigurable_params_

private

Auto-reconfigurable parameters for dynamic reconfiguration.

Definition at line 240 of file point_cloud_fusion.hpp.

cloud_publisher_

std::shared_ptr<point_cloud_transport::Publisher> point_cloud_fusion::PointCloudFusion::cloud_publisher_

private

Publisher.

Definition at line 255 of file point_cloud_fusion.hpp.

cloud_subscriber_callback_groups_

std::vector<rclcpp::CallbackGroup::SharedPtr> point_cloud_fusion::PointCloudFusion::cloud_subscriber_callback_groups_

private

Definition at line 244 of file point_cloud_fusion.hpp.

cloud_subscribers_

std::vector<std::shared_ptr<point_cloud_transport::SubscriberFilter> > point_cloud_fusion::PointCloudFusion::cloud_subscribers_

private

Definition at line 243 of file point_cloud_fusion.hpp.

config_mutex_

std::shared_mutex point_cloud_fusion::PointCloudFusion::config_mutex_

mutableprivate

Definition at line 241 of file point_cloud_fusion.hpp.

fixed_points_per_input_cloud_

int64_t point_cloud_fusion::PointCloudFusion::fixed_points_per_input_cloud_ = 0

private

Definition at line 221 of file point_cloud_fusion.hpp.

input_topics_

std::vector<std::string> point_cloud_fusion::PointCloudFusion::input_topics_

private

Definition at line 234 of file point_cloud_fusion.hpp.

input_transport_hints_

std::vector<std::string> point_cloud_fusion::PointCloudFusion::input_transport_hints_

private

Definition at line 235 of file point_cloud_fusion.hpp.

kAllowedOutputStampModes

const char* point_cloud_fusion::PointCloudFusion::kAllowedOutputStampModes = "latest, earliest, mean, input0"

staticconstexprprivate

Definition at line 206 of file point_cloud_fusion.hpp.

kDefaultTransportHint

const char* point_cloud_fusion::PointCloudFusion::kDefaultTransportHint = "raw"

staticconstexprprivate

Definition at line 205 of file point_cloud_fusion.hpp.

kMaxFixedPointsPerInputCloud

int64_t point_cloud_fusion::PointCloudFusion::kMaxFixedPointsPerInputCloud = 10000000

staticconstexprprivate

Definition at line 202 of file point_cloud_fusion.hpp.

kMaxInputTopics

std::size_t point_cloud_fusion::PointCloudFusion::kMaxInputTopics = 9

staticconstexprprivate

Definition at line 204 of file point_cloud_fusion.hpp.

kMaxOutputQueueSize

int32_t point_cloud_fusion::PointCloudFusion::kMaxOutputQueueSize = 1000

staticconstexprprivate

Definition at line 199 of file point_cloud_fusion.hpp.

kMaxRangeXY

double point_cloud_fusion::PointCloudFusion::kMaxRangeXY = 1000.0

staticconstexprprivate

Definition at line 208 of file point_cloud_fusion.hpp.

kMaxRangeZ

double point_cloud_fusion::PointCloudFusion::kMaxRangeZ = 20.0

staticconstexprprivate

Definition at line 210 of file point_cloud_fusion.hpp.

kMaxSyncQueueSize

int32_t point_cloud_fusion::PointCloudFusion::kMaxSyncQueueSize = 1000

staticconstexprprivate

Definition at line 196 of file point_cloud_fusion.hpp.

kMinFixedPointsPerInputCloud

int64_t point_cloud_fusion::PointCloudFusion::kMinFixedPointsPerInputCloud = 0

staticconstexprprivate

Definition at line 201 of file point_cloud_fusion.hpp.

kMinOutputQueueSize

int32_t point_cloud_fusion::PointCloudFusion::kMinOutputQueueSize = 1

staticconstexprprivate

Definition at line 198 of file point_cloud_fusion.hpp.

kMinRangeXY

double point_cloud_fusion::PointCloudFusion::kMinRangeXY = -1000.0

staticconstexprprivate

Definition at line 207 of file point_cloud_fusion.hpp.

kMinRangeZ

double point_cloud_fusion::PointCloudFusion::kMinRangeZ = -20.0

staticconstexprprivate

Definition at line 209 of file point_cloud_fusion.hpp.

kMinSyncQueueSize

int32_t point_cloud_fusion::PointCloudFusion::kMinSyncQueueSize = 1

staticconstexprprivate

Definition at line 195 of file point_cloud_fusion.hpp.

kStepSizeFixedPointsPerInputCloud

int64_t point_cloud_fusion::PointCloudFusion::kStepSizeFixedPointsPerInputCloud = 100

staticconstexprprivate

Definition at line 203 of file point_cloud_fusion.hpp.

kStepSizeOutputQueueSize

int32_t point_cloud_fusion::PointCloudFusion::kStepSizeOutputQueueSize = 1

staticconstexprprivate

Definition at line 200 of file point_cloud_fusion.hpp.

kStepSizeSyncQueueSize

int32_t point_cloud_fusion::PointCloudFusion::kStepSizeSyncQueueSize = 1

staticconstexprprivate

Definition at line 197 of file point_cloud_fusion.hpp.

max_time_diff_sec_

double point_cloud_fusion::PointCloudFusion::max_time_diff_sec_ = 0.05

private

ROS parameters.

Definition at line 215 of file point_cloud_fusion.hpp.

output_fields_

std::vector<std::string> point_cloud_fusion::PointCloudFusion::output_fields_

private

Definition at line 233 of file point_cloud_fusion.hpp.

output_queue_size_

int64_t point_cloud_fusion::PointCloudFusion::output_queue_size_ = 10

private

Definition at line 218 of file point_cloud_fusion.hpp.

output_stamp_mode_

OutputStampMode point_cloud_fusion::PointCloudFusion::output_stamp_mode_ = OutputStampMode::Earliest

private

Definition at line 230 of file point_cloud_fusion.hpp.

output_stamp_mode_param_

std::string point_cloud_fusion::PointCloudFusion::output_stamp_mode_param_ = "earliest"

private

Definition at line 231 of file point_cloud_fusion.hpp.

parameters_callback_

OnSetParametersCallbackHandle::SharedPtr point_cloud_fusion::PointCloudFusion::parameters_callback_

private

Callback handle for dynamic parameter reconfiguration.

Definition at line 250 of file point_cloud_fusion.hpp.

range_limits_enable_

bool point_cloud_fusion::PointCloudFusion::range_limits_enable_ = false

private

Definition at line 222 of file point_cloud_fusion.hpp.

range_limits_x_max_

double point_cloud_fusion::PointCloudFusion::range_limits_x_max_ = 1000.0

private

Definition at line 224 of file point_cloud_fusion.hpp.

range_limits_x_min_

double point_cloud_fusion::PointCloudFusion::range_limits_x_min_ = -1000.0

private

Definition at line 223 of file point_cloud_fusion.hpp.

range_limits_y_max_

double point_cloud_fusion::PointCloudFusion::range_limits_y_max_ = 1000.0

private

Definition at line 226 of file point_cloud_fusion.hpp.

range_limits_y_min_

double point_cloud_fusion::PointCloudFusion::range_limits_y_min_ = -1000.0

private

Definition at line 225 of file point_cloud_fusion.hpp.

range_limits_z_max_

double point_cloud_fusion::PointCloudFusion::range_limits_z_max_ = 20.0

private

Definition at line 228 of file point_cloud_fusion.hpp.

range_limits_z_min_

double point_cloud_fusion::PointCloudFusion::range_limits_z_min_ = -20.0

private

Definition at line 227 of file point_cloud_fusion.hpp.

setup_timer_

rclcpp::TimerBase::SharedPtr point_cloud_fusion::PointCloudFusion::setup_timer_

private

Timer to delay setup.

Definition at line 266 of file point_cloud_fusion.hpp.

sync_queue_size_

int64_t point_cloud_fusion::PointCloudFusion::sync_queue_size_ = 3

private

Definition at line 217 of file point_cloud_fusion.hpp.

synchronizer_

std::shared_ptr<void> point_cloud_fusion::PointCloudFusion::synchronizer_

private

Definition at line 245 of file point_cloud_fusion.hpp.

target_frame_

std::string point_cloud_fusion::PointCloudFusion::target_frame_ = "base_link"

private

Definition at line 232 of file point_cloud_fusion.hpp.

tf_buffer_

std::shared_ptr<tf2_ros::Buffer> point_cloud_fusion::PointCloudFusion::tf_buffer_

private

TF2 buffer and transform listener.

Definition at line 260 of file point_cloud_fusion.hpp.

tf_listener_

std::shared_ptr<tf2_ros::TransformListener> point_cloud_fusion::PointCloudFusion::tf_listener_

private

Definition at line 261 of file point_cloud_fusion.hpp.

use_cuda_

bool point_cloud_fusion::PointCloudFusion::use_cuda_ = true

private

Definition at line 229 of file point_cloud_fusion.hpp.

---

The documentation for this class was generated from the following files:

• point_cloud_fusion/include/point_cloud_fusion/point_cloud_fusion.hpp
• point_cloud_fusion/src/point_cloud_fusion.cpp