Skip to content
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

[foxy-devel] transport scoped parameters #142

Closed
wants to merge 2 commits into from
Closed

[foxy-devel] transport scoped parameters #142

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
55cfc45
add transport scoped parameters for CompressedPublisher
bmegli Apr 4, 2023
3588d40
implementation without template
bmegli Apr 4, 2023
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
18 changes: 18 additions & 0 deletions compressed_image_transport/include/compressed_image_transport/compressed_publisher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,12 @@

#include <rclcpp/node.hpp>

#include <vector>

namespace compressed_image_transport {

using CompressedImage = sensor_msgs::msg::CompressedImage;
using ParameterEvent = rcl_interfaces::msg::ParameterEvent;

class CompressedPublisher : public image_transport::SimplePublisherPlugin<CompressedImage>
{
Expand Down Expand Up @@ -80,6 +83,21 @@ class CompressedPublisher : public image_transport::SimplePublisherPlugin<Compre

Config config_;
rclcpp::Logger logger_;

private:
std::vector<std::string> deprecatedParameters_;
rclcpp::Subscription<ParameterEvent>::SharedPtr parameter_subscription_;

void onParameterEvent(ParameterEvent::SharedPtr event, std::string full_name);

void declareParameters(rclcpp::Node* node, const std::string& base_topic);

rclcpp::ParameterValue declareParameter(rclcpp::Node* node,
const std::string &base_name,
const std::string &transport_name,
const rclcpp::ParameterValue &default_value,
const rcl_interfaces::msg::ParameterDescriptor &descriptor);

};

} //namespace compressed_image_transport
169 changes: 115 additions & 54 deletions compressed_image_transport/src/compressed_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@

#include <rclcpp/exceptions/exceptions.hpp>
#include <rclcpp/parameter_client.hpp>
#include <rclcpp/parameter_events_filter.hpp>

#include <sstream>
#include <vector>
Expand All @@ -66,60 +67,7 @@ void CompressedPublisher::advertiseImpl(
typedef image_transport::SimplePublisherPlugin<sensor_msgs::msg::CompressedImage> Base;
Base::advertiseImpl(node, base_topic, custom_qos);

uint ns_len = node->get_effective_namespace().length();
std::string param_base_name = base_topic.substr(ns_len);
std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');
std::string format_param_name = param_base_name + ".format";
rcl_interfaces::msg::ParameterDescriptor format_description;
format_description.name = "format";
format_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
format_description.description = "Compression method";
format_description.read_only = false;
format_description.additional_constraints = "Supported values: [jpeg, png]";
try {
config_.format = node->declare_parameter(format_param_name, kDefaultFormat, format_description);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
RCLCPP_DEBUG(logger_, "%s was previously declared", format_param_name.c_str());
config_.format = node->get_parameter(format_param_name).get_value<std::string>();
}

std::string png_level_param_name = param_base_name + ".png_level";
rcl_interfaces::msg::ParameterDescriptor png_level_description;
png_level_description.name = "png_level";
png_level_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
png_level_description.description = "Compression level for PNG format";
png_level_description.read_only = false;
rcl_interfaces::msg::IntegerRange png_range;
png_range.from_value = 0;
png_range.to_value = 9;
png_range.step = 1;
png_level_description.integer_range.push_back(png_range);
try {
config_.png_level = node->declare_parameter(
png_level_param_name, kDefaultPngLevel, png_level_description);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
RCLCPP_DEBUG(logger_, "%s was previously declared", png_level_param_name.c_str());
config_.png_level = node->get_parameter(png_level_param_name).get_value<int64_t>();
}

std::string jpeg_quality_param_name = param_base_name + ".jpeg_quality";
rcl_interfaces::msg::ParameterDescriptor jpeg_quality_description;
jpeg_quality_description.name = "jpeg_quality";
jpeg_quality_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
jpeg_quality_description.description = "Image quality for JPEG format";
jpeg_quality_description.read_only = false;
rcl_interfaces::msg::IntegerRange jpeg_range;
jpeg_range.from_value = 1;
jpeg_range.to_value = 100;
jpeg_range.step = 1;
jpeg_quality_description.integer_range.push_back(jpeg_range);
try {
config_.jpeg_quality = node->declare_parameter(
jpeg_quality_param_name, kDefaultJpegQuality, jpeg_quality_description);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
RCLCPP_DEBUG(logger_, "%s was previously declared", jpeg_quality_param_name.c_str());
config_.jpeg_quality = node->get_parameter(jpeg_quality_param_name).get_value<int64_t>();
}
declareParameters(node, base_topic);
}

void CompressedPublisher::publish(
Expand Down Expand Up @@ -270,4 +218,117 @@ void CompressedPublisher::publish(
break;
}
}

void CompressedPublisher::declareParameters(rclcpp::Node* node, const std::string& base_topic)
{
uint ns_len = node->get_effective_namespace().length();
std::string param_base_name = base_topic.substr(ns_len);
std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');
const std::string transport_name = getTransportName();

using callbackT = std::function<void(ParameterEvent::SharedPtr event)>;
auto callback = std::bind(&CompressedPublisher::onParameterEvent, this, std::placeholders::_1, node->get_fully_qualified_name());

parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event<callbackT>(node, callback);

config_.format = declareParameter(node, param_base_name, transport_name, rclcpp::ParameterValue(kDefaultFormat),
rcl_interfaces::msg::ParameterDescriptor()
.set__name("format")
.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_STRING)
.set__description("Compression method")
.set__read_only(false)
.set__additional_constraints("Supported values: [jpeg, png]")).get<rclcpp::ParameterType::PARAMETER_STRING>();

config_.png_level = declareParameter(node, param_base_name, transport_name, rclcpp::ParameterValue(kDefaultPngLevel),
rcl_interfaces::msg::ParameterDescriptor()
.set__name("png_level")
.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER)
.set__description("Compression level for PNG format")
.set__read_only(false)
.set__integer_range(
{rcl_interfaces::msg::IntegerRange()
.set__from_value(0)
.set__to_value(9)
.set__step(1)})).get<rclcpp::ParameterType::PARAMETER_INTEGER>();

config_.jpeg_quality = declareParameter(node, param_base_name, transport_name, rclcpp::ParameterValue(kDefaultJpegQuality),
rcl_interfaces::msg::ParameterDescriptor()
.set__name("jpeg_quality")
.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER)
.set__description("Image quality for JPEG format")
.set__read_only(false)
.set__integer_range(
{rcl_interfaces::msg::IntegerRange()
.set__from_value(1)
.set__to_value(100)
.set__step(1)})).get<rclcpp::ParameterType::PARAMETER_INTEGER>();
}

rclcpp::ParameterValue CompressedPublisher::declareParameter(rclcpp::Node* node,
const std::string &base_name,
const std::string &transport_name,
const rclcpp::ParameterValue &default_value,
const rcl_interfaces::msg::ParameterDescriptor &descriptor)
{
rclcpp::ParameterValue param_value;

//transport scoped parameter (e.g. image_raw.compressed.format)
const std::string param_name = base_name + "." + transport_name + "." + descriptor.name;

try {
param_value = node->declare_parameter(param_name, default_value, descriptor);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
RCLCPP_DEBUG(logger_, "%s was previously declared", descriptor.name.c_str());
param_value = node->get_parameter(param_name).get_parameter_value();
}

rclcpp::ParameterValue value_set = param_value;

//deprecated non-scoped parameter name (e.g. image_raw.format)
const std::string deprecated_name = base_name + "." + descriptor.name;
deprecatedParameters_.push_back(deprecated_name);

// transport scoped parameter as default, otherwise we would overwrite
try {
param_value = node->declare_parameter(deprecated_name, param_value, descriptor);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
RCLCPP_DEBUG(logger_, "%s was previously declared", descriptor.name.c_str());
param_value = node->get_parameter(deprecated_name).get_parameter_value();
}

// in case parameter was set through deprecated keep transport scoped parameter in sync
if(value_set != param_value)
node->set_parameter(rclcpp::Parameter(param_name, param_value));

return param_value;
}

void CompressedPublisher::onParameterEvent(ParameterEvent::SharedPtr event, std::string full_name)
{
// filter out events from other nodes
if (event->node != full_name)
return;

// filter out new/changed deprecated parameters
using EventType = rclcpp::ParameterEventsFilter::EventType;

rclcpp::ParameterEventsFilter filter(event, deprecatedParameters_, {EventType::NEW, EventType::CHANGED});

if(!filter.get_events().size())
return;

// emit warnings for deprecated parameters
const std::string transport = getTransportName();

for (auto & it : filter.get_events())
{
const std::string name = it.second->name;
size_t dotIndex = name.find_last_of('.');
std::string recommended = name.substr(0, dotIndex + 1) + transport + name.substr(dotIndex);

RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated" <<
"; use transport qualified name `" << recommended << "`");
}
}

} //namespace compressed_image_transport