requested changes (not finished)

Author: JakubWarkocki

spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp

@@ -180,7 +180,7 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
   rclcpp::Service<spatio_temporal_voxel_layer::srv::SaveGrid>::SharedPtr _grid_saver;
   std::unique_ptr<rclcpp::Duration> _map_save_duration;
   rclcpp::Time _last_map_save_time;
-  std::string _global_frame, _footprint_frame;
+  std::string _global_frame, _robot_base_frame;
   double _voxel_size, _voxel_decay;
   int _combination_method, _mark_threshold;
   volume_grid::GlobalDecayModel _decay_model;
@@ -192,7 +192,6 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
 
   std::string _topics_string;
 
-
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
 };

spatio_temporal_voxel_layer/src/measurement_buffer.cpp

@@ -137,24 +137,7 @@ void MeasurementBuffer::BufferROSCloud(
       return;
     }
 
-   // transform the cloud in the global frame, directly or indirectly to apply relative z filter
     point_cloud_ptr cld_transformed(new sensor_msgs::msg::PointCloud2());
-
-    geometry_msgs::msg::TransformStamped tf_direct_global =
-      _buffer.lookupTransform(
-      _global_frame, cloud.header.frame_id,
-      tf2_ros::fromMsg(cloud.header.stamp));
-
-    geometry_msgs::msg::TransformStamped 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 =
-      _buffer.lookupTransform(
-      _global_frame, _z_reference_frame,
-      tf2_ros::fromMsg(cloud.header.stamp));
-
     pcl::PCLPointCloud2::Ptr cloud_pcl(new pcl::PCLPointCloud2());
     pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());
     pcl::PassThrough<pcl::PCLPointCloud2> pass_through_filter;
@@ -166,6 +149,10 @@ void MeasurementBuffer::BufferROSCloud(
     switch (_filter)
     {
     case Filters::PASSTHROUGH :
+      geometry_msgs::msg::TransformStamped tf_direct_global =
+        _buffer.lookupTransform(
+        _global_frame, cloud.header.frame_id,
+        tf2_ros::fromMsg(cloud.header.stamp));
       tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
       pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
       pass_through_filter.setInputCloud(cloud_pcl);
@@ -177,6 +164,10 @@ void MeasurementBuffer::BufferROSCloud(
       pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
       break;
     case Filters::VOXEL :
+      geometry_msgs::msg::TransformStamped tf_direct_global =
+        _buffer.lookupTransform(
+        _global_frame, cloud.header.frame_id,
+        tf2_ros::fromMsg(cloud.header.stamp));
       tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
       pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
       sor.setInputCloud(cloud_pcl);
@@ -189,6 +180,14 @@ void MeasurementBuffer::BufferROSCloud(
       pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed);
       break;
     case Filters::PASSTHROUGH_RELATIVE :
+      geometry_msgs::msg::TransformStamped 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 =
+        _buffer.lookupTransform(
+        _global_frame, _z_reference_frame,
+        tf2_ros::fromMsg(cloud.header.stamp));
       tf2::doTransform(cloud, *cld_transformed, tf_z_reference);
       pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
       pass_through_filter.setInputCloud(cloud_pcl);
@@ -201,6 +200,14 @@ 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 =
+        _buffer.lookupTransform(
+        _z_reference_frame, cloud.header.frame_id,
+        tf2_ros::fromMsg(cloud.header.stamp));
+      geometry_msgs::msg::TransformStamped tf_global_from_z_reference =
+        _buffer.lookupTransform(
+        _global_frame, _z_reference_frame,
+        tf2_ros::fromMsg(cloud.header.stamp));
       tf2::doTransform(cloud, *cld_transformed, tf_z_reference);
       pcl_conversions::toPCL(*cld_transformed, *cloud_pcl);
       sor.setInputCloud(cloud_pcl);
@@ -216,7 +223,6 @@ void MeasurementBuffer::BufferROSCloud(
     default:
       tf2::doTransform(cloud, *cld_transformed, tf_direct_global);
       break;
-    
     }
 
     _observation_list.front()._cloud.reset(cld_transformed.release());

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp

@@ -111,9 +111,9 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
   // use 3d transform for clearing footprint
   declareParameter("footprint_projection_enabled", rclcpp::ParameterValue(false));
   node->get_parameter(name_ + ".footprint_projection_enabled", _footprint_projection_enabled);
-  // footprint frame ( used only with footprint projection enabled )
-  declareParameter("footprint_frame", rclcpp::ParameterValue(std::string("")));
-  node->get_parameter(name_ + ".footprint_frame", _footprint_frame);
+  // robot base frame ( necessary for 3d footprint projection )
+  declareParameter("robot_base_frame", rclcpp::ParameterValue(std::string("")));
+  node->get_parameter(name_ + ".robot_base_frame", _robot_base_frame);
   // keep tabs on unknown space
   declareParameter(
     "track_unknown_space",
@@ -251,7 +251,6 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     node->get_parameter(name_ + "." + source + "." + "model_type", model_type_int);
     ModelType model_type = static_cast<ModelType>(model_type_int);
 
-
     if (filter_str == "passthrough") {
       RCLCPP_INFO(logger_, "Passthough filter activated.");
       filter = buffer::Filters::PASSTHROUGH;
@@ -569,17 +568,15 @@ bool SpatioTemporalVoxelLayer::updateFootprint(
     try {
       geometry_msgs::msg::TransformStamped tf_footprint_stamped =
         tf_->lookupTransform(
-          _global_frame, _footprint_frame,
-          rclcpp::Time(0)); 
-      
-      _transformed_footprint = getFootprint();
-      for (unsigned int i = 0; i < _transformed_footprint.size(); i++) {
-        tf2::doTransform(_transformed_footprint[i], _transformed_footprint[i], tf_footprint_stamped);       
+          _global_frame, _robot_base_frame,
+          rclcpp::Clock.now()); 
+      std::vector<geometry_msgs::Point> temp_footprint = getFootprint();
+      for (unsigned int i = 0; i < temp_footprint.size(); i++) {
+        tf2::doTransform(temp_footprint[i], temp_footprint[i], tf_footprint_stamped);       
         touch(
-          _transformed_footprint[i].x, _transformed_footprint[i].y,
+          temp_footprint[i].x, temp_footprint[i].y,
           min_x, min_y, max_x, max_y);
       }
-
     } catch (tf2::TransformException &ex) {
       RCLCPP_WARN(
         rclcpp::get_logger("SpatioTemporalVoxelLayer"),