Skip to content

Feature/support asymmetric lidar #341

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: jazzy
Choose a base branch
from
Open

Feature/support asymmetric lidar #341

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c0bb300
implemented support of vertical_fov_offset param for lidar with asymm…
pbrandschutz Oct 27, 2025
26958f5
added config example for Livox MID360
pbrandschutz Oct 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 46 additions & 0 deletions spatio_temporal_voxel_layer/example/mid360.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
4 changes: 2 additions & 2 deletions ...er/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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;
Expand Down
4 changes: 3 additions & 1 deletion spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_buffer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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);
Expand All @@ -158,7 +160,7 @@ class MeasurementBuffer
std::string _global_frame, _sensor_frame, _source_name, _topic_name;
std::list<observation::MeasurementReading> _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;
Expand Down
6 changes: 4 additions & 2 deletions spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_reading.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
/*****************************************************************************/
Expand All @@ -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),
Expand Down Expand Up @@ -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),
Expand All @@ -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;
};
Expand Down
16 changes: 9 additions & 7 deletions spatio_temporal_voxel_layer/src/frustum_models/three_dimensional_lidar_frustum.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)
{
Expand Down
15 changes: 11 additions & 4 deletions spatio_temporal_voxel_layer/src/measurement_buffer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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),
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
/*****************************************************************************/
Expand Down
2 changes: 1 addition & 1 deletion spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
15 changes: 13 additions & 2 deletions spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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));
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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())));
Expand Down Expand Up @@ -888,6 +891,14 @@ SpatioTemporalVoxelLayer::dynamicParametersCallback(std::vector<rclcpp::Paramete
buffer->Unlock();
}
}
} 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) {
Expand Down