Skip to content

Draft: lifecycle support for image_transport_plugins #1

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
authaldo wants to merge 4 commits into
base: rolling
Choose a base branch
from
Open

Draft: lifecycle support for image_transport_plugins #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2c8f586
ported compressed_image_transport to image_transport::node_interfaces
authaldo Feb 11, 2024
855f061
ported compressed_depth_image_transport to image_transport::node_inte…
authaldo Feb 11, 2024
d3041be
ported theora_image_transport to image_transport::node_interfaces
authaldo Feb 11, 2024
3a54139
ported zstd_image_transport to image_transport::node_interfaces
authaldo Feb 11, 2024
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
7 changes: 3 additions & 4 deletions ...pth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,10 +36,9 @@

#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <image_transport/node_interfaces.hpp>
#include <image_transport/simple_publisher_plugin.hpp>

#include <rclcpp/node.hpp>

#include <string>
#include <vector>

Expand All @@ -62,7 +61,7 @@ class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin<C
protected:
// Overridden to set up reconfigure server
void advertiseImpl(
rclcpp::Node * node,
image_transport::NodeInterfaces::SharedPtr node_interfaces,
const std::string &base_topic,
rmw_qos_profile_t custom_qos,
rclcpp::PublisherOptions options) override final;
Expand All @@ -71,7 +70,7 @@ class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin<C
const PublishFn& publish_fn) const override final;

rclcpp::Logger logger_;
rclcpp::Node * node_;
image_transport::NodeInterfaces::SharedPtr node_interfaces_;
private:
std::vector<std::string> parameters_;
std::vector<std::string> deprecatedParameters_;
Expand Down
32 changes: 17 additions & 15 deletions compressed_depth_image_transport/src/compressed_depth_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,26 +103,27 @@ const struct ParameterDefinition kParameters[] =
};

void CompressedDepthPublisher::advertiseImpl(
rclcpp::Node * node,
image_transport::NodeInterfaces::SharedPtr node_interfaces,
const std::string& base_topic,
rmw_qos_profile_t custom_qos,
rclcpp::PublisherOptions options)
{
node_ = node;
node_interfaces_ = node_interfaces;

typedef image_transport::SimplePublisherPlugin<sensor_msgs::msg::CompressedImage> Base;
Base::advertiseImpl(node, base_topic, custom_qos, options);
Base::advertiseImpl(node_interfaces_, base_topic, custom_qos, options);

// Declare Parameters
uint ns_len = node->get_effective_namespace().length();
// TODO: original implementation uses get_effective_namespace of rclcpp::Node
uint ns_len = strlen(node_interfaces->base->get_namespace());
std::string param_base_name = base_topic.substr(ns_len);
std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');

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

parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event<callbackT>(node, callback);
parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event<callbackT>(node_interfaces_->topics, callback);

for(const ParameterDefinition &pd : kParameters)
declareParameter(param_base_name, pd);
Expand All @@ -133,10 +134,10 @@ void CompressedDepthPublisher::publish(
const PublishFn& publish_fn) const
{
// Fresh Configuration
std::string cfg_format = node_->get_parameter(parameters_[FORMAT]).get_value<std::string>();
double cfg_depth_max = node_->get_parameter(parameters_[DEPTH_MAX]).get_value<double>();
double cfg_depth_quantization = node_->get_parameter(parameters_[DEPTH_QUANTIZATION]).get_value<double>();
int cfg_png_level = node_->get_parameter(parameters_[PNG_LEVEL]).get_value<int64_t>();
std::string cfg_format = node_interfaces_->parameters->get_parameter(parameters_[FORMAT]).get_value<std::string>();
double cfg_depth_max = node_interfaces_->parameters->get_parameter(parameters_[DEPTH_MAX]).get_value<double>();
double cfg_depth_quantization = node_interfaces_->parameters->get_parameter(parameters_[DEPTH_QUANTIZATION]).get_value<double>();
int cfg_png_level = node_interfaces_->parameters->get_parameter(parameters_[PNG_LEVEL]).get_value<int64_t>();

sensor_msgs::msg::CompressedImage::SharedPtr compressed_image =
encodeCompressedDepthImage(message,
Expand Down Expand Up @@ -165,15 +166,16 @@ void CompressedDepthPublisher::declareParameter(const std::string &base_name,
rclcpp::ParameterValue param_value;

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

// transport scoped parameter as default, otherwise we would overwrite
try {
node_->declare_parameter(deprecated_name, param_value, definition.descriptor);
node_interfaces_->parameters->declare_parameter(deprecated_name, param_value, definition.descriptor);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
RCLCPP_DEBUG(logger_, "%s was previously declared", definition.descriptor.name.c_str());
}
Expand Down Expand Up @@ -202,7 +204,7 @@ void CompressedDepthPublisher::onParameterEvent(ParameterEvent::SharedPtr event,
//e.g. `color.image_raw.` + `compressedDepth` + `png_level`
std::string recommendedName = name.substr(0, paramNameIndex + 1) + transport + name.substr(paramNameIndex);

rclcpp::Parameter recommendedValue = node_->get_parameter(recommendedName);
rclcpp::Parameter recommendedValue = node_interfaces_->parameters->get_parameter(recommendedName);

// do not emit warnings if deprecated value matches
if(it.second->value == recommendedValue.get_value_message())
Expand All @@ -211,7 +213,7 @@ void CompressedDepthPublisher::onParameterEvent(ParameterEvent::SharedPtr event,
RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated and ambiguous" <<
"; use transport qualified name `" << recommendedName << "`");

node_->set_parameter(rclcpp::Parameter(recommendedName, it.second->value));
node_interfaces_->parameters->set_parameters({rclcpp::Parameter(recommendedName, it.second->value)});
}
}

Expand Down
7 changes: 3 additions & 4 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 @@ -38,8 +38,7 @@
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <image_transport/simple_publisher_plugin.hpp>

#include <rclcpp/node.hpp>
#include <image_transport/node_interfaces.hpp>

#include "compressed_image_transport/compression_common.h"

Expand All @@ -62,7 +61,7 @@ class CompressedPublisher : public image_transport::SimplePublisherPlugin<Compre
protected:
// Overridden to set up reconfigure server
void advertiseImpl(
rclcpp::Node* node,
image_transport::NodeInterfaces::SharedPtr node_interfaces,
const std::string& base_topic,
rmw_qos_profile_t custom_qos,
rclcpp::PublisherOptions options) override;
Expand All @@ -71,7 +70,7 @@ class CompressedPublisher : public image_transport::SimplePublisherPlugin<Compre
const PublishFn& publish_fn) const override;

rclcpp::Logger logger_;
rclcpp::Node * node_;
image_transport::NodeInterfaces::SharedPtr node_interfaces_;

private:
std::vector<std::string> parameters_;
Expand Down
5 changes: 3 additions & 2 deletions compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <image_transport/simple_subscriber_plugin.hpp>
#include <image_transport/node_interfaces.hpp>

#include "compressed_image_transport/compression_common.h"

Expand All @@ -62,7 +63,7 @@ class CompressedSubscriber final : public image_transport::SimpleSubscriberPlugi
protected:

void subscribeImpl(
rclcpp::Node * ,
image_transport::NodeInterfaces::SharedPtr node_interfaces,
const std::string& base_topic,
const Callback& callback,
rmw_qos_profile_t custom_qos,
Expand All @@ -72,7 +73,7 @@ class CompressedSubscriber final : public image_transport::SimpleSubscriberPlugi
const Callback& user_cb) override;

rclcpp::Logger logger_;
rclcpp::Node * node_;
image_transport::NodeInterfaces::SharedPtr node_interfaces_;

private:
std::vector<std::string> parameters_;
Expand Down
35 changes: 18 additions & 17 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 @@ -130,25 +130,26 @@ const struct ParameterDefinition kParameters[] =
};

void CompressedPublisher::advertiseImpl(
rclcpp::Node* node,
image_transport::NodeInterfaces::SharedPtr node_interfaces,
const std::string& base_topic,
rmw_qos_profile_t custom_qos,
rclcpp::PublisherOptions options)
{
node_ = node;
node_interfaces_ = node_interfaces;
typedef image_transport::SimplePublisherPlugin<sensor_msgs::msg::CompressedImage> Base;
Base::advertiseImpl(node, base_topic, custom_qos, options);
Base::advertiseImpl(node_interfaces_, base_topic, custom_qos, options);

// Declare Parameters
uint ns_len = node->get_effective_namespace().length();
// TODO: original implementation uses get_effective_namespace of rclcpp::Node
uint ns_len = strlen(node_interfaces->base->get_namespace());
std::string param_base_name = base_topic.substr(ns_len);
std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');

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

parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event<callbackT>(node, callback);
parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event<callbackT>(node_interfaces_->topics, callback);

for(const ParameterDefinition &pd : kParameters)
declareParameter(param_base_name, pd);
Expand All @@ -159,12 +160,12 @@ void CompressedPublisher::publish(
const PublishFn& publish_fn) const
{
// Fresh Configuration
std::string cfg_format = node_->get_parameter(parameters_[FORMAT]).get_value<std::string>();
int cfg_png_level = node_->get_parameter(parameters_[PNG_LEVEL]).get_value<int64_t>();
int cfg_jpeg_quality = node_->get_parameter(parameters_[JPEG_QUALITY]).get_value<int64_t>();;
std::string cfg_tiff_res_unit = node_->get_parameter(parameters_[TIFF_RESOLUTION_UNIT]).get_value<std::string>();
int cfg_tiff_xdpi = node_->get_parameter(parameters_[TIFF_XDPI]).get_value<int64_t>();
int cfg_tiff_ydpi = node_->get_parameter(parameters_[TIFF_YDPI]).get_value<int64_t>();
std::string cfg_format = node_interfaces_->parameters->get_parameter(parameters_[FORMAT]).get_value<std::string>();
int cfg_png_level = node_interfaces_->parameters->get_parameter(parameters_[PNG_LEVEL]).get_value<int64_t>();
int cfg_jpeg_quality = node_interfaces_->parameters->get_parameter(parameters_[JPEG_QUALITY]).get_value<int64_t>();
std::string cfg_tiff_res_unit = node_interfaces_->parameters->get_parameter(parameters_[TIFF_RESOLUTION_UNIT]).get_value<std::string>();
int cfg_tiff_xdpi = node_interfaces_->parameters->get_parameter(parameters_[TIFF_XDPI]).get_value<int64_t>();
int cfg_tiff_ydpi = node_interfaces_->parameters->get_parameter(parameters_[TIFF_YDPI]).get_value<int64_t>();

// Compressed image message
sensor_msgs::msg::CompressedImage compressed;
Expand Down Expand Up @@ -400,15 +401,15 @@ void CompressedPublisher::declareParameter(const std::string &base_name,
rclcpp::ParameterValue param_value;

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

// transport scoped parameter as default, otherwise we would overwrite
try {
node_->declare_parameter(deprecated_name, param_value, definition.descriptor);
node_interfaces_->parameters->declare_parameter(deprecated_name, param_value, definition.descriptor);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
RCLCPP_DEBUG(logger_, "%s was previously declared", definition.descriptor.name.c_str());
}
Expand Down Expand Up @@ -437,7 +438,7 @@ void CompressedPublisher::onParameterEvent(ParameterEvent::SharedPtr event, std:
//e.g. `color.image_raw.` + `compressed` + `format`
std::string recommendedName = name.substr(0, paramNameIndex + 1) + transport + name.substr(paramNameIndex);

rclcpp::Parameter recommendedValue = node_->get_parameter(recommendedName);
rclcpp::Parameter recommendedValue = node_interfaces_->parameters->get_parameter(recommendedName);

// do not emit warnings if deprecated value matches
if(it.second->value == recommendedValue.get_value_message())
Expand All @@ -446,7 +447,7 @@ void CompressedPublisher::onParameterEvent(ParameterEvent::SharedPtr event, std:
RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated and ambiguous" <<
"; use transport qualified name `" << recommendedName << "`");

node_->set_parameter(rclcpp::Parameter(recommendedName, it.second->value));
node_interfaces_->parameters->set_parameters({rclcpp::Parameter(recommendedName, it.second->value)});
}
}

Expand Down
28 changes: 15 additions & 13 deletions compressed_image_transport/src/compressed_subscriber.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,27 +75,29 @@ const struct ParameterDefinition kParameters[] =
};

void CompressedSubscriber::subscribeImpl(
rclcpp::Node * node,
image_transport::NodeInterfaces::SharedPtr node_interfaces,
const std::string& base_topic,
const Callback& callback,
rmw_qos_profile_t custom_qos,
rclcpp::SubscriptionOptions options)
{
node_ = node;
logger_ = node->get_logger();
node_interfaces_ = node_interfaces;
logger_ = node_interfaces->logging->get_logger();
typedef image_transport::SimpleSubscriberPlugin<CompressedImage> Base;
Base::subscribeImpl(node, base_topic, callback, custom_qos, options);
Base::subscribeImpl(node_interfaces_, base_topic, callback, custom_qos, options);

// Declare Parameters
uint ns_len = node->get_effective_namespace().length();
// TODO: original implementation uses get_effective_namespace of rclcpp::Node
uint ns_len = strlen(node_interfaces_->base->get_namespace());
std::string param_base_name = base_topic.substr(ns_len);
std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');

using paramCallbackT = std::function<void(ParameterEvent::SharedPtr event)>;
auto paramCallback = std::bind(&CompressedSubscriber::onParameterEvent, this, std::placeholders::_1,
node->get_fully_qualified_name(), param_base_name);
node_interfaces_->base->get_fully_qualified_name(), param_base_name);

parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event<paramCallbackT>(node, paramCallback);
parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event<paramCallbackT>(node_interfaces_->topics,
paramCallback);

for(const ParameterDefinition &pd : kParameters)
declareParameter(param_base_name, pd);
Expand Down Expand Up @@ -190,7 +192,7 @@ void CompressedSubscriber::internalCallback(const CompressedImage::ConstSharedPt

int CompressedSubscriber::imdecodeFlagFromConfig()
{
std::string mode = node_->get_parameter(parameters_[MODE]).get_value<std::string>();
std::string mode = node_interfaces_->parameters->get_parameter(parameters_[MODE]).get_value<std::string>();

if (mode == "unchanged")
return cv::IMREAD_UNCHANGED;
Expand Down Expand Up @@ -219,15 +221,15 @@ void CompressedSubscriber::declareParameter(const std::string &base_name,
rclcpp::ParameterValue param_value;

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

// transport scoped parameter as default, otherwise we would overwrite
try {
node_->declare_parameter(deprecated_name, param_value, definition.descriptor);
node_interfaces_->parameters->declare_parameter(deprecated_name, param_value, definition.descriptor);
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {
RCLCPP_DEBUG(logger_, "%s was previously declared", definition.descriptor.name.c_str());
}
Expand Down Expand Up @@ -256,7 +258,7 @@ void CompressedSubscriber::onParameterEvent(ParameterEvent::SharedPtr event, std
//e.g. `color.image_raw.` + `compressed` + `format`
std::string recommendedName = name.substr(0, paramNameIndex + 1) + transport + name.substr(paramNameIndex);

rclcpp::Parameter recommendedValue = node_->get_parameter(recommendedName);
rclcpp::Parameter recommendedValue = node_interfaces_->parameters->get_parameter(recommendedName);

// do not emit warnings if deprecated value matches
if(it.second->value == recommendedValue.get_value_message())
Expand All @@ -265,7 +267,7 @@ void CompressedSubscriber::onParameterEvent(ParameterEvent::SharedPtr event, std
RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated and ambiguous" <<
"; use transport qualified name `" << recommendedName << "`");

node_->set_parameter(rclcpp::Parameter(recommendedName, it.second->value));
node_interfaces_->parameters->set_parameters({rclcpp::Parameter(recommendedName, it.second->value)});
}
}

Expand Down
7 changes: 3 additions & 4 deletions theora_image_transport/include/theora_image_transport/theora_publisher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,11 +34,10 @@

#include "theora_image_transport/compression_common.h"

#include <rclcpp/node.hpp>

#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>

#include <image_transport/node_interfaces.hpp>
#include <image_transport/simple_publisher_plugin.hpp>
#include <cv_bridge/cv_bridge.hpp>
#include <std_msgs/msg/header.hpp>
Expand Down Expand Up @@ -66,7 +65,7 @@ class TheoraPublisher : public image_transport::SimplePublisherPlugin<theora_ima

protected:
void advertiseImpl(
rclcpp::Node* node,
image_transport::NodeInterfaces::SharedPtr node_interfaces,
const std::string &base_topic,
rmw_qos_profile_t custom_qos,
rclcpp::PublisherOptions options) override;
Expand Down Expand Up @@ -98,7 +97,7 @@ class TheoraPublisher : public image_transport::SimplePublisherPlugin<theora_ima
mutable std::vector<theora_image_transport::msg::Packet> stream_header_;

rclcpp::Logger logger_;
rclcpp::Node * node_;
image_transport::NodeInterfaces::SharedPtr node_interfaces_;

private:
std::vector<std::string> parameters_;
Expand Down
Loading