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 new file mode 100644 index 00000000..222c73d1 --- /dev/null +++ b/spatio_temporal_voxel_layer/example/uneven_terrain_config.yaml @@ -0,0 +1,47 @@ +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 + footprint_projection_enabled: true # default off, strongly recommended when using 3d IMU data + 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 + 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 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 46df5378..459fef94 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/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 ea72e65f..7a7e7492 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.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/buffer_core.h" - +#include "tf2_ros/buffer.h" namespace spatio_temporal_voxel_layer { @@ -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, @@ -180,11 +183,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, _robot_base_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; diff --git a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp index 10c160bd..7656868b 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,98 @@ 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 = - _buffer.lookupTransform( - _global_frame, cloud.header.frame_id, - tf2_ros::fromMsg(cloud.header.stamp)); - tf2::doTransform(cloud, *cld_global, tf_stamped); - + point_cloud_ptr cld_transformed(new sensor_msgs::msg::PointCloud2()); 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; + + geometry_msgs::msg::TransformStamped tf_direct_global, tf_z_reference, tf_global_from_z_reference; + + + switch (_filter) + { + case Filters::PASSTHROUGH : + 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); + 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 : + 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); 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 : + tf_z_reference = + _buffer.lookupTransform( + _z_reference_frame, cloud.header.frame_id, + tf2_ros::fromMsg(cloud.header.stamp)); + 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); 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 : + tf_z_reference = + _buffer.lookupTransform( + _z_reference_frame, cloud.header.frame_id, + tf2_ros::fromMsg(cloud.header.stamp)); + 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); + 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 3cb9fe39..e4e607bc 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); + // 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", @@ -158,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()); @@ -169,7 +177,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 +204,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 +240,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 @@ -248,7 +259,13 @@ void SpatioTemporalVoxelLayer::onInitialize(void) } else if (filter_str == "voxel") { RCLCPP_INFO(logger_, "Voxel filter activated."); filter = buffer::Filters::VOXEL; - } else { + } else if (filter_str == "passthrough_relative") { + RCLCPP_INFO(logger_, "Relative Passthough filter activated."); + filter = buffer::Filters::PASSTHROUGH_RELATIVE; + } else if (filter_str == "voxel_relative") { + RCLCPP_INFO(logger_, "Relative Voxel filter activated."); + filter = buffer::Filters::VOXEL_RELATIVE; + } else { RCLCPP_INFO(logger_, "No filters activated."); filter = buffer::Filters::NONE; } @@ -266,7 +283,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, @@ -535,17 +552,41 @@ 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 { + rclcpp::Time current_time = _clock->now(); + geometry_msgs::msg::TransformStamped tf_footprint_stamped = + tf_->lookupTransform( + _global_frame, _robot_base_frame, + current_time ); + 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;