From 92d84b279424501fd54436d2bf2b30fa6c3438c9 Mon Sep 17 00:00:00 2001 From: kuba Date: Thu, 19 Sep 2024 17:12:08 +0200 Subject: [PATCH 1/8] relative filters --- .../measurement_buffer.hpp | 7 +- .../src/measurement_buffer.cpp | 79 +++++++++++++++---- .../src/spatio_temporal_voxel_layer.cpp | 24 +++++- 3 files changed, 87 insertions(+), 23 deletions(-) diff --git a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index c9418aaf..48b06550 100644 --- a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -73,7 +73,9 @@ enum class Filters { NONE = 0, VOXEL = 1, - PASSTHROUGH = 2 + PASSTHROUGH = 2, + VOXEL_RELATIVE = 3, + PASSTHROUGH_RELATIVE = 4 }; // conveniences for line lengths @@ -95,6 +97,7 @@ class MeasurementBuffer tf2_ros::Buffer & tf, const std::string & global_frame, const std::string & sensor_frame, + const std::string & z_reference_frame, const double & tf_tolerance, const double & min_d, const double & max_d, @@ -155,7 +158,7 @@ class MeasurementBuffer const rclcpp::Duration _observation_keep_time, _expected_update_rate; rclcpp::Time _last_updated; boost::recursive_mutex _lock; - std::string _global_frame, _sensor_frame, _source_name, _topic_name; + std::string _global_frame, _sensor_frame, _source_name, _topic_name, _z_reference_frame; std::list _observation_list; double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance; double _min_z, _max_z, _vertical_fov, _vertical_fov_padding, _horizontal_fov; diff --git a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp index 2e1fcdd3..e1536978 100644 --- a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp +++ b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp @@ -54,7 +54,7 @@ MeasurementBuffer::MeasurementBuffer( const double & observation_keep_time, const double & expected_update_rate, const double & min_obstacle_height, const double & max_obstacle_height, const double & obstacle_range, tf2_ros::Buffer & tf, const std::string & global_frame, - const std::string & sensor_frame, const double & tf_tolerance, + const std::string & sensor_frame, const std::string & z_reference_frame, const double & tf_tolerance, const double & min_d, const double & max_d, const double & vFOV, const double & vFOVPadding, const double & hFOV, const double & decay_acceleration, const bool & marking, @@ -67,7 +67,7 @@ MeasurementBuffer::MeasurementBuffer( _expected_update_rate(rclcpp::Duration::from_seconds(expected_update_rate)), _last_updated(clock->now()), _global_frame(global_frame), _sensor_frame(sensor_frame), _source_name(source_name), - _topic_name(topic_name), _min_obstacle_height(min_obstacle_height), + _topic_name(topic_name), _z_reference_frame(z_reference_frame), _min_obstacle_height(min_obstacle_height), _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range), _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d), _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding), @@ -137,44 +137,89 @@ void MeasurementBuffer::BufferROSCloud( return; } - // transform the cloud in the global frame - point_cloud_ptr cld_global(new sensor_msgs::msg::PointCloud2()); - geometry_msgs::msg::TransformStamped tf_stamped = + // 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)); - tf2::doTransform(cloud, *cld_global, tf_stamped); + + 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 pass_through_filter; + pcl::VoxelGrid sor; + float v_s = static_cast(_voxel_size); // 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 - if (_filter == Filters::VOXEL) { - pcl_conversions::toPCL(*cld_global, *cloud_pcl); - pcl::VoxelGrid sor; + + switch (_filter) + { + case Filters::PASSTHROUGH : + tf2::doTransform(cloud, *cld_transformed, tf_direct_global); + pcl_conversions::toPCL(*cld_transformed, *cloud_pcl); + pass_through_filter.setInputCloud(cloud_pcl); + pass_through_filter.setKeepOrganized(false); + pass_through_filter.setFilterFieldName("z"); + pass_through_filter.setFilterLimits( + _min_obstacle_height, _max_obstacle_height); + pass_through_filter.filter(*cloud_filtered); + pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed); + break; + case Filters::VOXEL : + tf2::doTransform(cloud, *cld_transformed, tf_direct_global); + pcl_conversions::toPCL(*cld_transformed, *cloud_pcl); sor.setInputCloud(cloud_pcl); sor.setFilterFieldName("z"); sor.setFilterLimits(_min_obstacle_height, _max_obstacle_height); sor.setDownsampleAllData(false); - float v_s = static_cast(_voxel_size); sor.setLeafSize(v_s, v_s, v_s); sor.setMinimumPointsNumberPerVoxel(static_cast(_voxel_min_points)); sor.filter(*cloud_filtered); - pcl_conversions::fromPCL(*cloud_filtered, *cld_global); - } else if (_filter == Filters::PASSTHROUGH) { - pcl_conversions::toPCL(*cld_global, *cloud_pcl); - pcl::PassThrough pass_through_filter; + pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed); + break; + case Filters::PASSTHROUGH_RELATIVE : + tf2::doTransform(cloud, *cld_transformed, tf_z_reference); + pcl_conversions::toPCL(*cld_transformed, *cloud_pcl); pass_through_filter.setInputCloud(cloud_pcl); pass_through_filter.setKeepOrganized(false); pass_through_filter.setFilterFieldName("z"); pass_through_filter.setFilterLimits( _min_obstacle_height, _max_obstacle_height); pass_through_filter.filter(*cloud_filtered); - pcl_conversions::fromPCL(*cloud_filtered, *cld_global); + pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed); + tf2::doTransform(*cld_transformed, *cld_transformed, tf_global_from_z_reference); + break; + case Filters::VOXEL_RELATIVE : + tf2::doTransform(cloud, *cld_transformed, tf_z_reference); + pcl_conversions::toPCL(*cld_transformed, *cloud_pcl); + sor.setInputCloud(cloud_pcl); + sor.setFilterFieldName("z"); + sor.setFilterLimits(_min_obstacle_height, _max_obstacle_height); + sor.setDownsampleAllData(false); + sor.setLeafSize(v_s, v_s, v_s); + sor.setMinimumPointsNumberPerVoxel(static_cast(_voxel_min_points)); + sor.filter(*cloud_filtered); + pcl_conversions::fromPCL(*cloud_filtered, *cld_transformed); + tf2::doTransform(*cld_transformed, *cld_transformed, tf_global_from_z_reference); + break; + default: + tf2::doTransform(cloud, *cld_transformed, tf_direct_global); + break; + } - _observation_list.front()._cloud.reset(cld_global.release()); + _observation_list.front()._cloud.reset(cld_transformed.release()); } catch (tf2::TransformException & ex) { // if fails, remove the empty observation _observation_list.pop_front(); diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp index d3c88031..e0a0110c 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp @@ -169,7 +169,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height; double min_z, max_z, vFOV, vFOVPadding; double hFOV, decay_acceleration, obstacle_range; - std::string topic, sensor_frame, data_type, filter_str; + std::string topic, sensor_frame, z_reference_frame, data_type, filter_str; bool inf_is_valid = false, clearing, marking; bool clear_after_reading, enabled; int voxel_min_points; @@ -196,6 +196,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) declareParameter(source + "." + "horizontal_fov_angle", rclcpp::ParameterValue(1.04)); declareParameter(source + "." + "decay_acceleration", rclcpp::ParameterValue(0.0)); declareParameter(source + "." + "filter", rclcpp::ParameterValue(std::string("passthrough"))); + declareParameter(source + "." + "z_reference_frame", rclcpp::ParameterValue(_global_frame)); declareParameter(source + "." + "voxel_min_points", rclcpp::ParameterValue(0)); declareParameter(source + "." + "clear_after_reading", rclcpp::ParameterValue(false)); declareParameter(source + "." + "enabled", rclcpp::ParameterValue(true)); @@ -231,6 +232,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void) node->get_parameter(name_ + "." + source + "." + "decay_acceleration", decay_acceleration); // performs an approximate voxel filter over the data to reduce node->get_parameter(name_ + "." + source + "." + "filter", filter_str); + // frame in which to calculate z bounds if relative filter is applied + node->get_parameter(name_ + "." + source + "." + "z_reference_frame", z_reference_frame); // minimum points per voxel for voxel filter node->get_parameter(name_ + "." + source + "." + "voxel_min_points", voxel_min_points); // clears measurement buffer after reading values from it @@ -242,15 +245,28 @@ void SpatioTemporalVoxelLayer::onInitialize(void) node->get_parameter(name_ + "." + source + "." + "model_type", model_type_int); ModelType model_type = static_cast(model_type_int); - if (filter_str == "passthrough") { + switch (filter) + { + case "passthrough": RCLCPP_INFO(logger_, "Passthough filter activated."); filter = buffer::Filters::PASSTHROUGH; - } else if (filter_str == "voxel") { + break; + case "voxel": RCLCPP_INFO(logger_, "Voxel filter activated."); filter = buffer::Filters::VOXEL; - } else { + break; + case "passthrough_relative": + RCLCPP_INFO(logger_, "Relative Passthough filter activated."); + filter = buffer::Filters::PASSTHROUGH_RELATIVE; + break; + case "voxel_relative": + RCLCPP_INFO(logger_, "Relative Voxel filter activated."); + filter = buffer::Filters::VOXEL_RELATIVE; + break; + default: RCLCPP_INFO(logger_, "No filters activated."); filter = buffer::Filters::NONE; + break; } if (!(data_type == "PointCloud2" || data_type == "LaserScan")) { From 62866bcc84c91aa9106c3306e00068ef49fb177f Mon Sep 17 00:00:00 2001 From: kuba Date: Thu, 19 Sep 2024 17:40:23 +0200 Subject: [PATCH 2/8] relative filters --- .../src/spatio_temporal_voxel_layer.cpp | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp index e0a0110c..6d73ae63 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp @@ -245,28 +245,22 @@ void SpatioTemporalVoxelLayer::onInitialize(void) node->get_parameter(name_ + "." + source + "." + "model_type", model_type_int); ModelType model_type = static_cast(model_type_int); - switch (filter) - { - case "passthrough": + + if (filter_str == "passthrough") { RCLCPP_INFO(logger_, "Passthough filter activated."); filter = buffer::Filters::PASSTHROUGH; - break; - case "voxel": + } else if (filter_str == "voxel") { RCLCPP_INFO(logger_, "Voxel filter activated."); filter = buffer::Filters::VOXEL; - break; - case "passthrough_relative": + } else if (filter_str == "passthrough_relative") { RCLCPP_INFO(logger_, "Relative Passthough filter activated."); filter = buffer::Filters::PASSTHROUGH_RELATIVE; - break; - case "voxel_relative": + } else if (filter_str == "voxel_relative") { RCLCPP_INFO(logger_, "Relative Voxel filter activated."); filter = buffer::Filters::VOXEL_RELATIVE; - break; - default: + } else { RCLCPP_INFO(logger_, "No filters activated."); filter = buffer::Filters::NONE; - break; } if (!(data_type == "PointCloud2" || data_type == "LaserScan")) { @@ -280,7 +274,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) new buffer::MeasurementBuffer( source, topic, observation_keep_time, expected_update_rate, min_obstacle_height, - max_obstacle_height, obstacle_range, *tf_, _global_frame, sensor_frame, + max_obstacle_height, obstacle_range, *tf_, _global_frame, sensor_frame, z_reference_frame, transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV, decay_acceleration, marking, clearing, _voxel_size, filter, voxel_min_points, enabled, clear_after_reading, model_type, From 44c17cb647864cdd19cb93becc591df92eabea95 Mon Sep 17 00:00:00 2001 From: kuba Date: Thu, 19 Sep 2024 17:49:12 +0200 Subject: [PATCH 3/8] example --- .../example/uneven_terrain_config.yaml | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml diff --git a/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml b/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml new file mode 100644 index 00000000..192dcfa0 --- /dev/null +++ b/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml @@ -0,0 +1,45 @@ +global_costmap: + global_costmap: + ros__parameters: + rgbd_obstacle_layer: + plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" + enabled: true + voxel_decay: 1.0 # seconds if linear, e^n if exponential + decay_model: 0 # 0=linear, 1=exponential, -1=persistent + voxel_size: 0.05 # meters + track_unknown_space: true # default space is known + mark_threshold: 0 # voxel height + update_footprint_enabled: true + combination_method: 1 # 1=max, 0=override + origin_z: 0.0 # meters + publish_voxel_map: false # default off + transform_tolerance: 0.2 # seconds + mapping_mode: false # default off, saves map not for navigation + map_save_duration: 60.0 # default 60s, how often to autosave + observation_sources: rgbd1_mark rgbd1_clear + rgbd1_mark: + data_type: PointCloud2 + topic: camera1/depth/points + marking: true + clearing: false + obstacle_range: 3.0 # meters + min_obstacle_height: 0.3 # default 0, meters + max_obstacle_height: 2.0 # default 3, meters + expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer + observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest + inf_is_valid: false # default false, for laser scans + filter: "passthrough_relative" # default passthrough, apply "voxel", "passthrough", their relative variants or no filter to sensor data, recommend on + z_reference_frame: "base_link" # if unspecified keeps using global frame for z filter, frame to calcualte the z coordinate before transforming pointcloud to global frame + voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter + clear_after_reading: true # default false, clear the buffer after the layer gets readings from it + rgbd1_clear: + data_type: PointCloud2 + topic: camera1/depth/points + marking: false + clearing: true + max_z: 7.0 # default 0, meters + min_z: 0.1 # default 10, meters + vertical_fov_angle: 0.8745 # default 0.7, radians + horizontal_fov_angle: 1.048 # default 1.04, radians + decay_acceleration: 1.0 # default 0, 1/s^2. If laser scanner MUST be 0 + model_type: 0 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar From bdda107b2f32de774f9006a03af46d3f02bdc1a2 Mon Sep 17 00:00:00 2001 From: kuba Date: Fri, 20 Sep 2024 17:39:09 +0200 Subject: [PATCH 4/8] footprint projection --- .../spatio_temporal_voxel_layer.hpp | 7 +-- .../src/spatio_temporal_voxel_layer.cpp | 50 +++++++++++++++---- 2 files changed, 45 insertions(+), 12 deletions(-) diff --git a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp index bc4a2ce0..115ba554 100644 --- a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp @@ -76,7 +76,7 @@ #include "message_filters/subscriber.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/buffer_core.h" - +#include "tf2_ros/buffer.h" namespace spatio_temporal_voxel_layer { @@ -180,11 +180,11 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer rclcpp::Service::SharedPtr _grid_saver; std::unique_ptr _map_save_duration; rclcpp::Time _last_map_save_time; - std::string _global_frame; + std::string _global_frame, _footprint_frame; double _voxel_size, _voxel_decay; int _combination_method, _mark_threshold; volume_grid::GlobalDecayModel _decay_model; - bool _update_footprint_enabled, _enabled; + bool _update_footprint_enabled, _footprint_projection_enabled, _enabled; std::vector _transformed_footprint; std::vector _static_observations; std::unique_ptr _voxel_grid; @@ -192,6 +192,7 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer std::string _topics_string; + // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler; }; diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp index 6d73ae63..2a60a8cc 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp @@ -78,7 +78,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) RCLCPP_INFO( logger_, "%s's global frame is %s.", getName().c_str(), _global_frame.c_str()); - + bool track_unknown_space; double transform_tolerance, map_save_time; int decay_model_int; @@ -108,6 +108,12 @@ void SpatioTemporalVoxelLayer::onInitialize(void) // clear under robot footprint declareParameter("update_footprint_enabled", rclcpp::ParameterValue(true)); node->get_parameter(name_ + ".update_footprint_enabled", _update_footprint_enabled); + // 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); // keep tabs on unknown space declareParameter( "track_unknown_space", @@ -543,17 +549,43 @@ bool SpatioTemporalVoxelLayer::updateFootprint( double * min_y, double * max_x, double * max_y) /*****************************************************************************/ { - // updates layer costmap to include footprint for clearing in voxel grid + // Updates layer costmap to include footprint for clearing in voxel grid if (!_update_footprint_enabled) { return false; } - nav2_costmap_2d::transformFootprint( - robot_x, robot_y, robot_yaw, - getFootprint(), _transformed_footprint); - for (unsigned int i = 0; i < _transformed_footprint.size(); i++) { - touch( - _transformed_footprint[i].x, _transformed_footprint[i].y, - min_x, min_y, max_x, max_y); + + if (!_footprint_projection_enabled) { + // Simple 2D transformation + nav2_costmap_2d::transformFootprint( + robot_x, robot_y, robot_yaw, + getFootprint(), _transformed_footprint); + for (unsigned int i = 0; i < _transformed_footprint.size(); i++) { + touch( + _transformed_footprint[i].x, _transformed_footprint[i].y, + min_x, min_y, max_x, max_y); + } + } else { + // Using tf2 for 3d rotation to provide accurate projection of the footprint + 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); + touch( + _transformed_footprint[i].x, _transformed_footprint[i].y, + min_x, min_y, max_x, max_y); + } + + } catch (tf2::TransformException &ex) { + RCLCPP_WARN( + rclcpp::get_logger("SpatioTemporalVoxelLayer"), + "Could not perform a transform for footprint projection: %s", ex.what()); + return false; + } } return true; From b30f90494acb6576a9c87455b6bfc9f1ae5533b6 Mon Sep 17 00:00:00 2001 From: kuba Date: Fri, 20 Sep 2024 17:43:03 +0200 Subject: [PATCH 5/8] footprint projection example --- spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml b/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml index 192dcfa0..9bef71c8 100644 --- a/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml +++ b/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml @@ -10,6 +10,8 @@ global_costmap: track_unknown_space: true # default space is known mark_threshold: 0 # voxel height update_footprint_enabled: true + footprint_projection_enabled: true # default off, strongly recommended when using 3d IMU data + footprint_frame: "base_link" # necessary for use with footprint projection, combination_method: 1 # 1=max, 0=override origin_z: 0.0 # meters publish_voxel_map: false # default off From 7583c6bdbd7fd9b74a4021ec19001c1dd34d81e9 Mon Sep 17 00:00:00 2001 From: kuba Date: Fri, 18 Oct 2024 20:47:06 +0200 Subject: [PATCH 6/8] requested changes (not finished) --- .../spatio_temporal_voxel_layer.hpp | 3 +- .../src/measurement_buffer.cpp | 42 +++++++++++-------- .../src/spatio_temporal_voxel_layer.cpp | 21 ++++------ 3 files changed, 34 insertions(+), 32 deletions(-) diff --git a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp index 115ba554..c5940513 100644 --- a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/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::SharedPtr _grid_saver; std::unique_ptr _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; }; diff --git a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp index e1536978..9d5af229 100644 --- a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp +++ b/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 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()); diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp index 2a60a8cc..0458e7e8 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/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(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 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"), From 22ef92d8a2ade2f48e7da239b48a65bd96168368 Mon Sep 17 00:00:00 2001 From: kuba Date: Sat, 19 Oct 2024 15:49:13 +0200 Subject: [PATCH 7/8] requested changes --- README.md | 9 ++++++--- .../example/uneven_terrain_config.yaml | 2 +- .../src/spatio_temporal_voxel_layer.cpp | 7 +++---- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 1f00929b..83150189 100644 --- a/README.md +++ b/README.md @@ -101,7 +101,7 @@ Required dependencies ROS Kinetic, navigation, OpenVDB, TBB. An example fully-described configuration is shown below. -Note: We supply two PCL filters within STVL to massage the data to lower compute overhead. STVL has an approximate voxel filter to make the data more sparse if very dense. It also has a passthrough filter to limit processing data within the valid minimum to maximum height bounds. The voxel filter is recommended if it lowers CPU overhead, otherwise, passthrough filter. No filter is also available if you pre-process your data or are not interested in performance optimizations. +Note: We supply two PCL filters within STVL to massage the data to lower compute overhead. STVL has an approximate voxel filter to make the data more sparse if very dense. It also has a passthrough filter to limit processing data within the valid minimum to maximum height bounds. The voxel filter is recommended if it lowers CPU overhead, otherwise, passthrough filter. No filter is also available if you pre-process your data or are not interested in performance optimizations. Filters have "relative" variants made to use robot's frame for obstacle classification instead of the global frame in case of navigating sloped surfaces. ``` rgbd_obstacle_layer: @@ -113,7 +113,9 @@ rgbd_obstacle_layer: observation_persistence: 0.0 #seconds max_obstacle_height: 2.0 #meters mark_threshold: 0 #voxel height - update_footprint_enabled: true + update_footprint_enabled: true + footprint_projection_enabled: true #default off, recommended when using 3d IMU data. Uses tf2 to transform the footprint + robot_base_frame: "base_link" #frame to use with footprint_projection_enabled combination_method: 1 #1=max, 0=override obstacle_range: 3.0 #meters origin_z: 0.0 #meters @@ -133,7 +135,8 @@ rgbd_obstacle_layer: observation_persistence: 0.0 #default 0, use all measurements taken during now-value, 0=latest inf_is_valid: false #default false, for laser scans clear_after_reading: true #default false, clear the buffer after the layer gets readings from it - filter: "voxel" #default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommended to have at one filter on + filter: "voxel" #default passthrough, apply "voxel", "passthrough", "voxel_relative", "passthrough_relative", or no filter to sensor data, recommended to have at one filter on + z_reference_frame: "base_link" #default global_frame, use with *_relative filters as a frame for obstacle classification based on z coordinate voxel_min_points: 0 #default 0, minimum points per voxel for voxel filter rgbd1_clear: enabled: true #default true, can be toggled on/off with associated service call diff --git a/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml b/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml index 9bef71c8..222c73d1 100644 --- a/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml +++ b/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml @@ -11,7 +11,7 @@ global_costmap: mark_threshold: 0 # voxel height update_footprint_enabled: true footprint_projection_enabled: true # default off, strongly recommended when using 3d IMU data - footprint_frame: "base_link" # necessary for use with footprint projection, + robot_base_frame: "base_link" # necessary for use with footprint projection, combination_method: 1 # 1=max, 0=override origin_z: 0.0 # meters publish_voxel_map: false # default off diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp index 0458e7e8..ffe7411a 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp @@ -570,11 +570,10 @@ bool SpatioTemporalVoxelLayer::updateFootprint( tf_->lookupTransform( _global_frame, _robot_base_frame, rclcpp::Clock.now()); - std::vector temp_footprint = getFootprint(); - for (unsigned int i = 0; i < temp_footprint.size(); i++) { - tf2::doTransform(temp_footprint[i], temp_footprint[i], tf_footprint_stamped); + for (unsigned int i = 0; i < _transformed_footprint.size(); i++) { + tf2::doTransform(_transformed_footprint[i], _transformed_footprint[i], tf_footprint_stamped); touch( - temp_footprint[i].x, temp_footprint[i].y, + _transformed_footprint[i].x, _transformed_footprint[i].y, min_x, min_y, max_x, max_y); } } catch (tf2::TransformException &ex) { From 4a2c80e0dc084fb0b08016403e230bf54754bd8b Mon Sep 17 00:00:00 2001 From: kuba Date: Sat, 19 Oct 2024 20:56:21 +0200 Subject: [PATCH 8/8] bugfix --- .../spatio_temporal_voxel_layer.hpp | 3 +++ .../src/measurement_buffer.cpp | 15 +++++++++------ .../src/spatio_temporal_voxel_layer.cpp | 5 ++++- 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp index c5940513..cde1cfd1 100644 --- a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/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 resp); private: + // clock + rclcpp::Clock::SharedPtr _clock; + // Sensor callbacks void LaserScanCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr message, diff --git a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp index 9d5af229..0a1ada27 100644 --- a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp +++ b/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)); diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp index ffe7411a..4d7d5a81 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/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(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(