diff --git a/spatio_temporal_voxel_layer/example/mid360.yaml b/spatio_temporal_voxel_layer/example/mid360.yaml new file mode 100644 index 00000000..ab222ac5 --- /dev/null +++ b/spatio_temporal_voxel_layer/example/mid360.yaml @@ -0,0 +1,46 @@ +global_costmap: + global_costmap: + ros__parameters: + rgbd_obstacle_layer: + plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" + enabled: true + voxel_decay: 15.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: /livox/lidar + 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: "voxel" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on 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: + enabled: true #default true, can be toggled on/off with associated service call + data_type: PointCloud2 + topic: /livox/lidar + marking: false + clearing: true + max_z: 8.0 # default 0, meters + min_z: 1.0 # default 10, meters + vertical_fov_angle: 1.8151 # default 0.7, radians. For 3D lidars it's the symmetric FOV about the planar axis. + vertical_fov_offset: 0.3927 # default 0, radians. For 3D lidars it's the offset from the planar axis. f.e. a MID360 has a vFOV of -7deg to 52deg, so the offset is +22.5deg (0.3927 rad) + vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters + horizontal_fov_angle: 6.29 # 3D lidar scanners like the VLP16 have 360 deg horizontal FOV. + decay_acceleration: 5.0 # default 0, 1/s^2. + model_type: 1 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar \ No newline at end of file diff --git a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp index 9a773703..0c86e834 100644 --- a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp +++ b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp @@ -53,7 +53,7 @@ class ThreeDimensionalLidarFrustum : public Frustum { public: ThreeDimensionalLidarFrustum( - const double & vFOV, const double & vFOVPadding, + const double & vFOV, const double & vFOVOffset, const double & vFOVPadding, const double & hFOV, const double & min_dist, const double & max_dist); virtual ~ThreeDimensionalLidarFrustum(void); @@ -72,7 +72,7 @@ class ThreeDimensionalLidarFrustum : public Frustum double Dot(const VectorWithPt3D &, const openvdb::Vec3d &) const; double Dot(const VectorWithPt3D &, const Eigen::Vector3d &) const; - double _vFOV, _vFOVPadding, _hFOV, _min_d, _max_d; + double _vFOV, _vFOVOffset, _vFOVPadding, _hFOV, _min_d, _max_d; double _hFOVhalf; double _min_d_squared, _max_d_squared; double _tan_vFOVhalf; 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..c654a12d 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 @@ -99,6 +99,7 @@ class MeasurementBuffer const double & min_d, const double & max_d, const double & vFOV, + const double & vFOVOffset, const double & vFOVPadding, const double & hFOV, const double & decay_acceleration, @@ -133,6 +134,7 @@ class MeasurementBuffer void SetMaxObstacleHeight(const double & max_obstacle_height); void SetMinZ(const double & min_z); void SetMaxZ(const double & max_z); + void SetVerticalFovOffset(const double & vertical_fov_offset); void SetVerticalFovPadding(const double & vertical_fov_padding); void SetHorizontalFovAngle(const double & horizontal_fov_angle); void SetVerticalFovAngle(const double & vertical_fov_angle); @@ -158,7 +160,7 @@ class MeasurementBuffer std::string _global_frame, _sensor_frame, _source_name, _topic_name; 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; + double _min_z, _max_z, _vertical_fov, _vertical_fov_offset, _vertical_fov_padding, _horizontal_fov; double _decay_acceleration, _voxel_size; bool _marking, _clearing; Filters _filter; diff --git a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_reading.h b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_reading.h index 60071c2a..a51cce44 100644 --- a/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_reading.h +++ b/spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_reading.h @@ -69,7 +69,7 @@ struct MeasurementReading /*****************************************************************************/ MeasurementReading( geometry_msgs::msg::Point & origin, sensor_msgs::msg::PointCloud2 cloud, - double obstacle_range, double min_z, double max_z, double vFOV, + double obstacle_range, double min_z, double max_z, double vFOV, double vFOVOffset, double vFOVPadding, double hFOV, double decay_acceleration, bool marking, bool clearing, ModelType model_type) /*****************************************************************************/ @@ -79,6 +79,7 @@ struct MeasurementReading _min_z_in_m(min_z), _max_z_in_m(max_z), _vertical_fov_in_rad(vFOV), + _vertical_fov_offset_in_rad(vFOVOffset), _vertical_fov_padding_in_m(vFOVPadding), _horizontal_fov_in_rad(hFOV), _marking(marking), @@ -106,6 +107,7 @@ struct MeasurementReading _min_z_in_m(obs._min_z_in_m), _max_z_in_m(obs._max_z_in_m), _vertical_fov_in_rad(obs._vertical_fov_in_rad), + _vertical_fov_offset_in_rad(obs._vertical_fov_offset_in_rad), _vertical_fov_padding_in_m(obs._vertical_fov_padding_in_m), _horizontal_fov_in_rad(obs._horizontal_fov_in_rad), _marking(obs._marking), @@ -119,7 +121,7 @@ struct MeasurementReading geometry_msgs::msg::Quaternion _orientation; std::shared_ptr < sensor_msgs::msg::PointCloud2 > _cloud; double _obstacle_range_in_m, _min_z_in_m, _max_z_in_m; - double _vertical_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; + double _vertical_fov_in_rad, _vertical_fov_offset_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; double _marking, _clearing, _decay_acceleration; ModelType _model_type; }; diff --git a/spatio_temporal_voxel_layer/src/frustum_models/three_dimensional_lidar_frustum.cpp b/spatio_temporal_voxel_layer/src/frustum_models/three_dimensional_lidar_frustum.cpp index e3337152..7f3ac1cb 100644 --- a/spatio_temporal_voxel_layer/src/frustum_models/three_dimensional_lidar_frustum.cpp +++ b/spatio_temporal_voxel_layer/src/frustum_models/three_dimensional_lidar_frustum.cpp @@ -42,10 +42,10 @@ namespace geometry /*****************************************************************************/ ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum( - const double & vFOV, const double & vFOVPadding, const double & hFOV, - const double & min_dist, const double & max_dist) -: _vFOV(vFOV), _vFOVPadding(vFOVPadding), _hFOV(hFOV), - _min_d(min_dist), _max_d(max_dist) + const double & vFOV, const double & vFOVOffset, const double & vFOVPadding, + const double & hFOV, const double & min_dist, const double & max_dist) +: _vFOV(vFOV), _vFOVOffset(vFOVOffset), _vFOVPadding(vFOVPadding), + _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist) /*****************************************************************************/ { _hFOVhalf = _hFOV / 2.0; @@ -84,16 +84,18 @@ bool ThreeDimensionalLidarFrustum::IsInside(const openvdb::Vec3d & pt) const double radial_distance_squared = (transformed_pt[0] * transformed_pt[0]) + (transformed_pt[1] * transformed_pt[1]); - + // Check if inside frustum valid range if (radial_distance_squared > _max_d_squared || radial_distance_squared < _min_d_squared) { return false; } + + // Check if inside frustum valid vFOV + const double z_rel = transformed_pt[2] - sqrt(radial_distance_squared) * std::tan(_vFOVOffset); + const double v_padded = fabs(z_rel) + _vFOVPadding; - // // Check if inside frustum valid vFOV - const double v_padded = fabs(transformed_pt[2]) + _vFOVPadding; if (( v_padded * v_padded / radial_distance_squared) > _tan_vFOVhalf_squared) { diff --git a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp index 10c160bd..9c5932b7 100644 --- a/spatio_temporal_voxel_layer/src/measurement_buffer.cpp +++ b/spatio_temporal_voxel_layer/src/measurement_buffer.cpp @@ -56,7 +56,7 @@ MeasurementBuffer::MeasurementBuffer( const double & obstacle_range, tf2_ros::Buffer & tf, const std::string & global_frame, const std::string & sensor_frame, const double & tf_tolerance, const double & min_d, const double & max_d, const double & vFOV, - const double & vFOVPadding, const double & hFOV, + const double & vFOVOffset, const double & vFOVPadding, const double & hFOV, const double & decay_acceleration, const bool & marking, const bool & clearing, const double & voxel_size, const Filters & filter, const int & voxel_min_points, const bool & enabled, @@ -70,7 +70,7 @@ MeasurementBuffer::MeasurementBuffer( _topic_name(topic_name), _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), + _vertical_fov(vFOV), _vertical_fov_offset(vFOVOffset), _vertical_fov_padding(vFOVPadding), _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), _voxel_size(voxel_size), _marking(marking), _clearing(clearing), _filter(filter), _voxel_min_points(voxel_min_points), @@ -124,8 +124,8 @@ void MeasurementBuffer::BufferROSCloud( _observation_list.front()._min_z_in_m = _min_z; _observation_list.front()._max_z_in_m = _max_z; _observation_list.front()._vertical_fov_in_rad = _vertical_fov; - _observation_list.front()._vertical_fov_padding_in_m = - _vertical_fov_padding; + _observation_list.front()._vertical_fov_offset_in_rad = _vertical_fov_offset; + _observation_list.front()._vertical_fov_padding_in_m = _vertical_fov_padding; _observation_list.front()._horizontal_fov_in_rad = _horizontal_fov; _observation_list.front()._decay_acceleration = _decay_acceleration; _observation_list.front()._clearing = _clearing; @@ -317,6 +317,13 @@ void MeasurementBuffer::SetVerticalFovAngle(const double & vertical_fov_angle) _vertical_fov = vertical_fov_angle; } +/*****************************************************************************/ +void MeasurementBuffer::SetVerticalFovOffset(const double & vertical_fov_offset) +/*****************************************************************************/ +{ + _vertical_fov_offset = vertical_fov_offset; +} + /*****************************************************************************/ void MeasurementBuffer::SetVerticalFovPadding(const double & vertical_fov_padding) /*****************************************************************************/ diff --git a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp index 1102a957..df6acb7a 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp +++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp @@ -129,7 +129,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums( it->_horizontal_fov_in_rad, it->_min_z_in_m, it->_max_z_in_m); } else if (it->_model_type == THREE_DIMENSIONAL_LIDAR) { frustum = new geometry::ThreeDimensionalLidarFrustum( - it->_vertical_fov_in_rad, it->_vertical_fov_padding_in_m, + it->_vertical_fov_in_rad, it->_vertical_fov_offset_in_rad, it->_vertical_fov_padding_in_m, it->_horizontal_fov_in_rad, it->_min_z_in_m, it->_max_z_in_m); } else { // add else if statement for each implemented model 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 caa8a957..0be28233 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp @@ -170,7 +170,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) while (ss >> source) { // get the parameters for the specific topic double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height; - double min_z, max_z, vFOV, vFOVPadding; + double min_z, max_z, vFOV, vFOVOffset, vFOVPadding; double hFOV, decay_acceleration, obstacle_range; std::string topic, sensor_frame, data_type, filter_str; bool inf_is_valid = false, clearing, marking; @@ -195,6 +195,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) declareParameter(source + "." + "min_z", rclcpp::ParameterValue(0.0)); declareParameter(source + "." + "max_z", rclcpp::ParameterValue(10.0)); declareParameter(source + "." + "vertical_fov_angle", rclcpp::ParameterValue(0.7)); + declareParameter(source + "." + "vertical_fov_offset", rclcpp::ParameterValue(0.0)); declareParameter(source + "." + "vertical_fov_padding", rclcpp::ParameterValue(0.0)); declareParameter(source + "." + "horizontal_fov_angle", rclcpp::ParameterValue(1.04)); declareParameter(source + "." + "decay_acceleration", rclcpp::ParameterValue(0.0)); @@ -226,6 +227,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void) node->get_parameter(name_ + "." + source + "." + "max_z", max_z); // vertical FOV angle in rad node->get_parameter(name_ + "." + source + "." + "vertical_fov_angle", vFOV); + // vertical FOV offset angle in rad + node->get_parameter(name_ + "." + source + "." + "vertical_fov_offset", vFOVOffset); // vertical FOV padding in meters (3D lidar frustum only) node->get_parameter(name_ + "." + source + "." + "vertical_fov_padding", vFOVPadding); // horizontal FOV angle in rad @@ -268,7 +271,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source, topic, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, obstacle_range, *tf_, _global_frame, sensor_frame, - transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV, + transform_tolerance, min_z, max_z, vFOV, vFOVOffset, vFOVPadding, hFOV, decay_acceleration, marking, clearing, _voxel_size, filter, voxel_min_points, enabled, clear_after_reading, model_type, node->get_clock(), node->get_logger()))); @@ -888,6 +891,14 @@ SpatioTemporalVoxelLayer::dynamicParametersCallback(std::vectorUnlock(); } } + } else if (name == name_ + "." + source + "." + "vertical_fov_offset") { + for (auto & buffer : _observation_buffers) { + if (buffer->GetSourceName() == source) { + buffer->Lock(); + buffer->SetVerticalFovOffset(parameter.as_double()); + buffer->Unlock(); + } + } } else if (name == name_ + "." + source + "." + "vertical_fov_padding") { for (auto & buffer : _observation_buffers) { if (buffer->GetSourceName() == source) {