Skip to content

Commit

Permalink
republisher: qos override pub and sub (#88)
Browse files Browse the repository at this point in the history 
Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
  • Loading branch information
@ahcorde
ahcorde authored Jun 3, 2024
1 parent 8e13f6c commit 7ecf416
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 20 deletions.
38 changes: 22 additions & 16 deletions point_cloud_transport/include/point_cloud_transport/point_cloud_transport.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,10 +211,10 @@ class PointCloudTransport : public PointCloudTransportLoader
const std::string & base_topic, rmw_qos_profile_t custom_qos,
const std::function<void(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &)> & callback,
const VoidPtr & tracked_object = {},
const point_cloud_transport::TransportHints * transport_hints = nullptr)
const point_cloud_transport::TransportHints * transport_hints = nullptr,
rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions())
{
(void)tracked_object;
rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions();
return Subscriber(
node_, base_topic, callback, sub_loader_,
getTransportOrDefault(transport_hints), custom_qos, options);
Expand All @@ -226,62 +226,66 @@ class PointCloudTransport : public PointCloudTransportLoader
const std::string & base_topic, uint32_t queue_size,
const std::function<void(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &)> & callback,
const VoidPtr & tracked_object = {},
const point_cloud_transport::TransportHints * transport_hints = nullptr)
const point_cloud_transport::TransportHints * transport_hints = nullptr,
rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions())
{
rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data;
custom_qos.depth = queue_size;
return subscribe(
base_topic, custom_qos, callback, tracked_object, transport_hints);
base_topic, custom_qos, callback, tracked_object, transport_hints, options);
}

//! Subscribe to a point cloud topic, version for bare function.
POINT_CLOUD_TRANSPORT_PUBLIC
point_cloud_transport::Subscriber subscribe(
const std::string & base_topic, rmw_qos_profile_t custom_qos,
void (* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &),
const point_cloud_transport::TransportHints * transport_hints = nullptr)
const point_cloud_transport::TransportHints * transport_hints = nullptr,
rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions())
{
return subscribe(
base_topic, custom_qos,
std::function<void(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &)>(fp),
VoidPtr(), transport_hints);
VoidPtr(), transport_hints, options);
}

POINT_CLOUD_TRANSPORT_PUBLIC
point_cloud_transport::Subscriber subscribe(
const std::string & base_topic, uint32_t queue_size,
void (* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &),
const point_cloud_transport::TransportHints * transport_hints = nullptr)
const point_cloud_transport::TransportHints * transport_hints = nullptr,
rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions())
{
return subscribe(
base_topic, queue_size,
std::function<void(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &)>(fp),
VoidPtr(), transport_hints);
VoidPtr(), transport_hints, options);
}

//! Subscribe to a point cloud topic, version for class member function with bare pointer.
template<class T>
point_cloud_transport::Subscriber subscribe(
const std::string & base_topic, rmw_qos_profile_t custom_qos,
void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const, T * obj,
const point_cloud_transport::TransportHints * transport_hints = nullptr)
const point_cloud_transport::TransportHints * transport_hints = nullptr,
rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions())
{
return subscribe(
base_topic, custom_qos, std::bind(
fp,
obj, std::placeholders::_1), VoidPtr(), transport_hints);
obj, std::placeholders::_1), VoidPtr(), transport_hints, options);
}

template<class T>
point_cloud_transport::Subscriber subscribe(
const std::string & base_topic, uint32_t queue_size,
void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const, T * obj,
const point_cloud_transport::TransportHints * transport_hints = nullptr)
const point_cloud_transport::TransportHints * transport_hints = nullptr,
const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions())
{
return subscribe(
base_topic, queue_size, std::bind(
fp,
obj, std::placeholders::_1), VoidPtr(), transport_hints);
obj, std::placeholders::_1), VoidPtr(), transport_hints, options);
}

//! Subscribe to a point cloud topic, version for class member function with shared_ptr.
Expand All @@ -290,7 +294,8 @@ class PointCloudTransport : public PointCloudTransportLoader
const std::string & base_topic, rmw_qos_profile_t custom_qos,
void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const,
const std::shared_ptr<T> & obj,
const point_cloud_transport::TransportHints * transport_hints = nullptr)
const point_cloud_transport::TransportHints * transport_hints = nullptr,
const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions())
{
return subscribe(
base_topic, custom_qos, std::bind(
Expand All @@ -303,12 +308,13 @@ class PointCloudTransport : public PointCloudTransportLoader
const std::string & base_topic, uint32_t queue_size,
void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const,
const std::shared_ptr<T> & obj,
const point_cloud_transport::TransportHints * transport_hints = nullptr)
const point_cloud_transport::TransportHints * transport_hints = nullptr,
const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions())
{
return subscribe(
base_topic, queue_size, std::bind(
fp,
obj, std::placeholders::_1), obj, transport_hints);
obj, std::placeholders::_1), obj, transport_hints, options);
}

private:
Expand Down
21 changes: 17 additions & 4 deletions point_cloud_transport/src/republish.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,13 +121,26 @@ void Republisher::initialize()

pct = std::make_shared<point_cloud_transport::PointCloudTransport>(this->shared_from_this());

auto qos_override_options = rclcpp::QosOverridingOptions(
{
rclcpp::QosPolicyKind::Depth,
rclcpp::QosPolicyKind::Durability,
rclcpp::QosPolicyKind::History,
rclcpp::QosPolicyKind::Reliability,
});
rclcpp::SubscriptionOptions sub_options;
rclcpp::PublisherOptions pub_options;
pub_options.qos_overriding_options = qos_override_options;
sub_options.qos_overriding_options = qos_override_options;

if (out_transport.empty()) {
// Use all available transports for output
this->simple_pub =
std::make_shared<point_cloud_transport::Publisher>(
pct->advertise(
out_topic,
rmw_qos_profile_default));
rmw_qos_profile_default,
pub_options));

RCLCPP_INFO_STREAM(
this->get_logger(),
Expand All @@ -142,7 +155,7 @@ void Republisher::initialize()
const point_cloud_transport::TransportHints hint(in_transport);
this->sub = pct->subscribe(
in_topic, static_cast<uint32_t>(1),
pub_mem_fn, this->simple_pub, &hint);
pub_mem_fn, this->simple_pub, &hint, sub_options);
} else {
// Load transport plugin
typedef point_cloud_transport::PublisherPlugin Plugin;
Expand All @@ -153,7 +166,7 @@ void Republisher::initialize()
auto instance = loader->createUniqueInstance(lookup_name);
// DO NOT use instance after this line
this->pub = std::move(instance);
pub->advertise(this->shared_from_this(), out_topic);
pub->advertise(this->shared_from_this(), out_topic, rmw_qos_profile_default, pub_options);

RCLCPP_INFO_STREAM(
this->get_logger(),
Expand All @@ -170,7 +183,7 @@ void Republisher::initialize()
const point_cloud_transport::TransportHints hint(in_transport);
this->sub = pct->subscribe(
in_topic, static_cast<uint32_t>(1),
pub_mem_fn, pub, &hint);
pub_mem_fn, pub, &hint, sub_options);
}
RCLCPP_INFO_STREAM(
this->get_logger(),
Expand Down

0 comments on commit 7ecf416

Please sign in to comment.