bugfix

Author: JakubWarkocki

spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp

@@ -135,6 +135,9 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
     std::shared_ptr<spatio_temporal_voxel_layer::srv::SaveGrid::Response> resp);
 
 private:
+  // clock
+  rclcpp::Clock::SharedPtr _clock;
+
   // Sensor callbacks
   void LaserScanCallback(
     sensor_msgs::msg::LaserScan::ConstSharedPtr message,

spatio_temporal_voxel_layer/src/measurement_buffer.cpp

@@ -146,10 +146,13 @@ void MeasurementBuffer::BufferROSCloud(
     // remove points that are below or above our height restrictions, and
     // in the same time, remove NaNs and if user wants to use it, combine with a
 
+    geometry_msgs::msg::TransformStamped tf_direct_global, tf_z_reference, tf_global_from_z_reference;
+
+
     switch (_filter)
     {
     case Filters::PASSTHROUGH :
-      geometry_msgs::msg::TransformStamped tf_direct_global =
+      tf_direct_global =
         _buffer.lookupTransform(
         _global_frame, cloud.header.frame_id,
         tf2_ros::fromMsg(cloud.header.stamp));
@@ -164,7 +167,7 @@ void MeasurementBuffer::BufferROSCloud(
       pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
       break;
     case Filters::VOXEL :
-      geometry_msgs::msg::TransformStamped tf_direct_global =
+      tf_direct_global =
         _buffer.lookupTransform(
         _global_frame, cloud.header.frame_id,
         tf2_ros::fromMsg(cloud.header.stamp));
@@ -180,11 +183,11 @@ void MeasurementBuffer::BufferROSCloud(
       pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
       break;
     case Filters::PASSTHROUGH_RELATIVE :
-      geometry_msgs::msg::TransformStamped tf_z_reference =
+      tf_z_reference =
         _buffer.lookupTransform(
         _z_reference_frame, cloud.header.frame_id,
         tf2_ros::fromMsg(cloud.header.stamp));
-      geometry_msgs::msg::TransformStamped tf_global_from_z_reference =
+      tf_global_from_z_reference =
         _buffer.lookupTransform(
         _global_frame, _z_reference_frame,
         tf2_ros::fromMsg(cloud.header.stamp));
@@ -200,11 +203,11 @@ void MeasurementBuffer::BufferROSCloud(
       tf2::doTransform(*cld_transformed, *cld_transformed, tf_global_from_z_reference);
       break;
     case Filters::VOXEL_RELATIVE :
-      geometry_msgs::msg::TransformStamped tf_z_reference =
+      tf_z_reference =
         _buffer.lookupTransform(
         _z_reference_frame, cloud.header.frame_id,
         tf2_ros::fromMsg(cloud.header.stamp));
-      geometry_msgs::msg::TransformStamped tf_global_from_z_reference =
+      tf_global_from_z_reference =
         _buffer.lookupTransform(
         _global_frame, _z_reference_frame,
         tf2_ros::fromMsg(cloud.header.stamp));

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp

@@ -164,6 +164,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     node->get_clock(), _voxel_size, static_cast<double>(default_value_), _decay_model,
     _voxel_decay, _publish_voxels);
 
+  _clock = node->get_clock();
+
   matchSize();
 
   RCLCPP_INFO(logger_, "%s created underlying voxel grid.", getName().c_str());
@@ -566,10 +568,11 @@ bool SpatioTemporalVoxelLayer::updateFootprint(
   } else {
     // Using tf2 for 3d rotation to provide accurate projection of the footprint
     try {
+      rclcpp::Time current_time = _clock->now();
       geometry_msgs::msg::TransformStamped tf_footprint_stamped =
         tf_->lookupTransform(
           _global_frame, _robot_base_frame,
-          rclcpp::Clock.now()); 
+          current_time ); 
       for (unsigned int i = 0; i < _transformed_footprint.size(); i++) {
         tf2::doTransform(_transformed_footprint[i], _transformed_footprint[i], tf_footprint_stamped);       
         touch(