From 5c6a332232b8cfbeddf6c81e1ec103df8a5a6619 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 16 Jan 2026 11:43:10 +0000 Subject: [PATCH] Use the new declare_or_get_parameter API for nav2_costmap_2d Signed-off-by: mini-1235 --- .../src/spatio_temporal_voxel_layer.cpp | 163 +++++++++--------- 1 file changed, 77 insertions(+), 86 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 f74bde1..576f951 100644 --- a/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp +++ b/spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp @@ -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(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()); @@ -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(model_type_int); if (filter_str == "passthrough") {