Skip to content

Use the new declare_or_get_parameter API for nav2_costmap_2d #350

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

Merged
SteveMacenski merged 1 commit into from
Jan 16, 2026
Merged

Use the new declare_or_get_parameter API for nav2_costmap_2d #350

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
163 changes: 77 additions & 86 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 @@ -85,50 +85,49 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
int decay_model_int;
// source names
auto node = node_.lock();
declareParameter("observation_sources", rclcpp::ParameterValue(std::string("")));
node->get_parameter(name_ + ".observation_sources", _topics_string);
_topics_string = node->declare_or_get_parameter(
name_ + ".observation_sources", std::string(""));
// timeout in seconds for transforms
declareParameter("transform_tolerance", rclcpp::ParameterValue(0.2));
node->get_parameter(name_ + ".transform_tolerance", transform_tolerance);
transform_tolerance = node->declare_or_get_parameter(
name_ + ".transform_tolerance", 0.2);
// whether to default on
declareParameter("enabled", rclcpp::ParameterValue(true));
node->get_parameter(name_ + ".enabled", _enabled);
_enabled = node->declare_or_get_parameter(
name_ + ".enabled", true);
enabled_ = _enabled;
// publish the voxel grid to visualize
declareParameter("publish_voxel_map", rclcpp::ParameterValue(false));
node->get_parameter(name_ + ".publish_voxel_map", _publish_voxels);
_publish_voxels = node->declare_or_get_parameter(
name_ + ".publish_voxel_map", false);
// size of each voxel in meters
declareParameter("voxel_size", rclcpp::ParameterValue(0.05));
node->get_parameter(name_ + ".voxel_size", _voxel_size);
_voxel_size = node->declare_or_get_parameter(
name_ + ".voxel_size", 0.05);
// 1=takes highest in layers, 0=takes current layer
declareParameter("combination_method", rclcpp::ParameterValue(1));
node->get_parameter(name_ + ".combination_method", _combination_method);
_combination_method = node->declare_or_get_parameter(
name_ + ".combination_method", 1);
// number of voxels per vertical needed to have obstacle
declareParameter("mark_threshold", rclcpp::ParameterValue(0));
node->get_parameter(name_ + ".mark_threshold", _mark_threshold);
_mark_threshold = node->declare_or_get_parameter(
name_ + ".mark_threshold", 0);
// clear under robot footprint
declareParameter("update_footprint_enabled", rclcpp::ParameterValue(true));
node->get_parameter(name_ + ".update_footprint_enabled", _update_footprint_enabled);
_update_footprint_enabled = node->declare_or_get_parameter(
name_ + ".update_footprint_enabled", true);
// keep tabs on unknown space
declareParameter(
"track_unknown_space",
rclcpp::ParameterValue(layered_costmap_->isTrackingUnknown()));
node->get_parameter(name_ + ".track_unknown_space", track_unknown_space);
declareParameter("decay_model", rclcpp::ParameterValue(0));
node->get_parameter(name_ + ".decay_model", decay_model_int);
track_unknown_space = node->declare_or_get_parameter(
name_ + ".track_unknown_space",
layered_costmap_->isTrackingUnknown());
decay_model_int = node->declare_or_get_parameter(
name_ + ".decay_model", 0);
_decay_model = static_cast<volume_grid::GlobalDecayModel>(decay_model_int);
// decay param
declareParameter("voxel_decay", rclcpp::ParameterValue(-1.0));
node->get_parameter(name_ + ".voxel_decay", _voxel_decay);
_voxel_decay = node->declare_or_get_parameter(
name_ + ".voxel_decay", -1.0);
// distance decay param
declareParameter("voxel_distance_decay", rclcpp::ParameterValue(-1.0));
node->get_parameter(name_ + ".voxel_distance_decay", _voxel_distance_decay);
_voxel_distance_decay = node->declare_or_get_parameter(
name_ + ".voxel_distance_decay", -1.0);
// whether to map or navigate
declareParameter("mapping_mode", rclcpp::ParameterValue(false));
node->get_parameter(name_ + ".mapping_mode", _mapping_mode);
_mapping_mode = node->declare_or_get_parameter(
name_ + ".mapping_mode", false);
// if mapping, how often to save a map for safety
declareParameter("map_save_duration", rclcpp::ParameterValue(60.0));
node->get_parameter(name_ + ".map_save_duration", map_save_time);
map_save_time = node->declare_or_get_parameter(
name_ + ".map_save_duration", 60.0);
RCLCPP_INFO(
logger_,
"%s loaded parameters from parameter server.", getName().c_str());
Expand Down Expand Up @@ -179,76 +178,68 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
int voxel_min_points;
buffer::Filters filter;

declareParameter(source + "." + "topic", rclcpp::ParameterValue(std::string("")));
declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));
declareParameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0));
declareParameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0));
declareParameter(
source + "." + "data_type",
rclcpp::ParameterValue(std::string("PointCloud2")));
declareParameter(source + "." + "transport_type", rclcpp::ParameterValue(std::string("raw")));
declareParameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));
declareParameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(3.0));
declareParameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));
declareParameter(source + "." + "marking", rclcpp::ParameterValue(true));
declareParameter(source + "." + "clearing", rclcpp::ParameterValue(false));
declareParameter(source + "." + "obstacle_range", rclcpp::ParameterValue(2.5));

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));
declareParameter(source + "." + "filter", rclcpp::ParameterValue(std::string("passthrough")));
declareParameter(source + "." + "voxel_min_points", rclcpp::ParameterValue(0));
declareParameter(source + "." + "clear_after_reading", rclcpp::ParameterValue(false));
declareParameter(source + "." + "enabled", rclcpp::ParameterValue(true));
declareParameter(source + "." + "model_type", rclcpp::ParameterValue(0));

node->get_parameter(name_ + "." + source + "." + "topic", topic);
node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame);
node->get_parameter(
name_ + "." + source + "." + "observation_persistence",
observation_keep_time);
node->get_parameter(
name_ + "." + source + "." + "expected_update_rate",
expected_update_rate);
node->get_parameter(name_ + "." + source + "." + "data_type", data_type);
node->get_parameter(name_ + "." + source + "." + "transport_type", transport_type);
node->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height);
node->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height);
node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid);
node->get_parameter(name_ + "." + source + "." + "marking", marking);
node->get_parameter(name_ + "." + source + "." + "clearing", clearing);
node->get_parameter(name_ + "." + source + "." + "obstacle_range", obstacle_range);
topic = node->declare_or_get_parameter(
name_ + "." + source + "." + "topic", std::string(""));
sensor_frame = node->declare_or_get_parameter(
name_ + "." + source + "." + "sensor_frame", std::string(""));
observation_keep_time = node->declare_or_get_parameter(
name_ + "." + source + "." + "observation_persistence", 0.0);
expected_update_rate = node->declare_or_get_parameter(
name_ + "." + source + "." + "expected_update_rate", 0.0);
data_type = node->declare_or_get_parameter(
name_ + "." + source + "." + "data_type", std::string("PointCloud2"));
transport_type = node->declare_or_get_parameter(
name_ + "." + source + "." + "transport_type", std::string("raw"));
min_obstacle_height = node->declare_or_get_parameter(
name_ + "." + source + "." + "min_obstacle_height", 0.0);
max_obstacle_height = node->declare_or_get_parameter(
name_ + "." + source + "." + "max_obstacle_height", 3.0);
inf_is_valid = node->declare_or_get_parameter(
name_ + "." + source + "." + "inf_is_valid", false);
marking = node->declare_or_get_parameter(
name_ + "." + source + "." + "marking", true);
clearing = node->declare_or_get_parameter(
name_ + "." + source + "." + "clearing", false);
obstacle_range = node->declare_or_get_parameter(
name_ + "." + source + "." + "obstacle_range", 2.5);

// minimum distance from camera it can see
node->get_parameter(name_ + "." + source + "." + "min_z", min_z);
min_z = node->declare_or_get_parameter(
name_ + "." + source + "." + "min_z", 0.0);
// maximum distance from camera it can see
node->get_parameter(name_ + "." + source + "." + "max_z", max_z);
max_z = node->declare_or_get_parameter(
name_ + "." + source + "." + "max_z", 10.0);
// vertical FOV angle in rad
node->get_parameter(name_ + "." + source + "." + "vertical_fov_angle", vFOV);
vFOV = node->declare_or_get_parameter(
name_ + "." + source + "." + "vertical_fov_angle", 0.7);
// vertical FOV offset angle in rad
node->get_parameter(name_ + "." + source + "." + "vertical_fov_offset", vFOVOffset);
vFOVOffset = node->declare_or_get_parameter(
name_ + "." + source + "." + "vertical_fov_offset", 0.0);
// vertical FOV padding in meters (3D lidar frustum only)
node->get_parameter(name_ + "." + source + "." + "vertical_fov_padding", vFOVPadding);
vFOVPadding = node->declare_or_get_parameter(
name_ + "." + source + "." + "vertical_fov_padding", 0.0);
// horizontal FOV angle in rad
node->get_parameter(name_ + "." + source + "." + "horizontal_fov_angle", hFOV);
hFOV = node->declare_or_get_parameter(
name_ + "." + source + "." + "horizontal_fov_angle", 1.04);
// acceleration scales the model's decay in presence of readings
node->get_parameter(name_ + "." + source + "." + "decay_acceleration", decay_acceleration);
decay_acceleration = node->declare_or_get_parameter(
name_ + "." + source + "." + "decay_acceleration", 0.0);
// performs an approximate voxel filter over the data to reduce
node->get_parameter(name_ + "." + source + "." + "filter", filter_str);
filter_str = node->declare_or_get_parameter(
name_ + "." + source + "." + "filter", std::string("passthrough"));
// minimum points per voxel for voxel filter
node->get_parameter(name_ + "." + source + "." + "voxel_min_points", voxel_min_points);
voxel_min_points = node->declare_or_get_parameter(
name_ + "." + source + "." + "voxel_min_points", 0);
// clears measurement buffer after reading values from it
node->get_parameter(name_ + "." + source + "." + "clear_after_reading", clear_after_reading);
clear_after_reading = node->declare_or_get_parameter(
name_ + "." + source + "." + "clear_after_reading", false);
// Whether the frustum is enabled on startup. Can be toggled with service
node->get_parameter(name_ + "." + source + "." + "enabled", enabled);
enabled = node->declare_or_get_parameter(
name_ + "." + source + "." + "enabled", true);
// model type - default depth camera frustum model
int model_type_int = 0;
node->get_parameter(name_ + "." + source + "." + "model_type", model_type_int);
model_type_int = node->declare_or_get_parameter(
name_ + "." + source + "." + "model_type", 0);
ModelType model_type = static_cast<ModelType>(model_type_int);

if (filter_str == "passthrough") {
Expand Down