Skip to content

Commit

Permalink
Updated node declare_parameter to new syntax (ros2#882)
Browse files Browse the repository at this point in the history
- also added required defaults for optional parameters (qos set)

Signed-off-by: Adam Dabrowski <[email protected]>
  • Loading branch information
adamdbrw authored Oct 13, 2021
1 parent 7cd143e commit 2d8fd5f
Showing 1 changed file with 21 additions and 20 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -28,13 +28,14 @@ void load_qos_configuration(
PublisherGroupConfig & group_config,
const std::string & group_prefix)
{
unsigned int qos_depth = 10;
std::string qos_reliability = "reliable";
std::string qos_durability = "volatile";
auto qos_prefix = group_prefix + ".qos";
node.declare_parameter(qos_prefix + ".qos_depth");
node.declare_parameter(qos_prefix + ".qos_reliability");
node.declare_parameter(qos_prefix + ".qos_durability");
node.declare_parameter<int>(qos_prefix + ".qos_depth", qos_depth);
node.declare_parameter<std::string>(qos_prefix + ".qos_reliability", qos_reliability);
node.declare_parameter<std::string>(qos_prefix + ".qos_durability", qos_durability);

unsigned int qos_depth = 10;
std::string qos_reliability, qos_durability;
node.get_parameter(qos_prefix + ".qos_depth", qos_depth);
node.get_parameter(qos_prefix + ".qos_reliability", qos_reliability);
node.get_parameter(qos_prefix + ".qos_durability", qos_durability);
Expand All @@ -50,7 +51,7 @@ void load_qos_configuration(
bool wait_for_subscriptions_from_node_parameters(rclcpp::Node & node)
{
const std::string parameters_ns = "publishers";
node.declare_parameter(parameters_ns + ".wait_for_subscriptions", true);
node.declare_parameter<bool>(parameters_ns + ".wait_for_subscriptions", true);
bool wait_for_subscriptions;
node.get_parameter(parameters_ns + ".wait_for_subscriptions", wait_for_subscriptions);
return wait_for_subscriptions;
Expand All @@ -62,15 +63,15 @@ std::vector<PublisherGroupConfig> publisher_groups_from_node_parameters(
std::vector<PublisherGroupConfig> configurations;
std::vector<std::string> publisher_groups;
const std::string parameters_ns = "publishers";
node.declare_parameter(parameters_ns + ".publisher_groups");
node.declare_parameter<std::vector<std::string>>(parameters_ns + ".publisher_groups");
node.get_parameter(parameters_ns + ".publisher_groups", publisher_groups);
for (const auto & group_name : publisher_groups) {
auto group_prefix = parameters_ns + "." + group_name;
node.declare_parameter(group_prefix + ".publishers_count");
node.declare_parameter(group_prefix + ".topic_root");
node.declare_parameter(group_prefix + ".msg_size_bytes");
node.declare_parameter(group_prefix + ".msg_count_each");
node.declare_parameter(group_prefix + ".rate_hz");
node.declare_parameter<int>(group_prefix + ".publishers_count");
node.declare_parameter<std::string>(group_prefix + ".topic_root");
node.declare_parameter<int>(group_prefix + ".msg_size_bytes");
node.declare_parameter<int>(group_prefix + ".msg_count_each");
node.declare_parameter<int>(group_prefix + ".rate_hz");

PublisherGroupConfig group_config;
node.get_parameter(
Expand Down Expand Up @@ -108,14 +109,14 @@ BagConfig bag_config_from_node_parameters(
const std::string default_bag_folder("/tmp/rosbag2_test");
BagConfig bag_config;

node.declare_parameter("storage_id", "sqlite3");
node.declare_parameter("max_cache_size", 10000000);
node.declare_parameter("max_bag_size", 0);
node.declare_parameter("db_folder", default_bag_folder);
node.declare_parameter("storage_config_file", "");
node.declare_parameter("compression_format", "");
node.declare_parameter("compression_queue_size", 1);
node.declare_parameter("compression_threads", 0);
node.declare_parameter<std::string>("storage_id", "sqlite3");
node.declare_parameter<int>("max_cache_size", 10000000);
node.declare_parameter<int>("max_bag_size", 0);
node.declare_parameter<std::string>("db_folder", default_bag_folder);
node.declare_parameter<std::string>("storage_config_file", "");
node.declare_parameter<std::string>("compression_format", "");
node.declare_parameter<int>("compression_queue_size", 1);
node.declare_parameter<int>("compression_threads", 0);

node.get_parameter("storage_id", bag_config.storage_options.storage_id);
node.get_parameter("max_cache_size", bag_config.storage_options.max_cache_size);
Expand Down

0 comments on commit 2d8fd5f

Please sign in to comment.