Added support for QoS profiles
This commit is contained in:
parent
3f2df48fef
commit
90f71aa28e
3 changed files with 14 additions and 14 deletions
|
@ -118,7 +118,8 @@ public:
|
||||||
/* Create and return a Publisher. */
|
/* Create and return a Publisher. */
|
||||||
template<typename MessageT>
|
template<typename MessageT>
|
||||||
rclcpp::publisher::Publisher::SharedPtr
|
rclcpp::publisher::Publisher::SharedPtr
|
||||||
create_publisher(const std::string & topic_name, size_t queue_size);
|
create_publisher(
|
||||||
|
const std::string & topic_name, const rmw_qos_profile_t & qos_profile);
|
||||||
|
|
||||||
/* Create and return a Subscription. */
|
/* Create and return a Subscription. */
|
||||||
|
|
||||||
|
@ -130,7 +131,7 @@ public:
|
||||||
typename rclcpp::subscription::Subscription<MessageT>::SharedPtr
|
typename rclcpp::subscription::Subscription<MessageT>::SharedPtr
|
||||||
create_subscription(
|
create_subscription(
|
||||||
const std::string & topic_name,
|
const std::string & topic_name,
|
||||||
size_t queue_size,
|
const rmw_qos_profile_t & qos_profile,
|
||||||
std::function<void(const std::shared_ptr<MessageT> &)> callback,
|
std::function<void(const std::shared_ptr<MessageT> &)> callback,
|
||||||
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
|
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
|
||||||
bool ignore_local_publications = false,
|
bool ignore_local_publications = false,
|
||||||
|
|
|
@ -76,11 +76,10 @@ Node::Node(const std::string & node_name, context::Context::SharedPtr context)
|
||||||
});
|
});
|
||||||
|
|
||||||
using rclcpp::callback_group::CallbackGroupType;
|
using rclcpp::callback_group::CallbackGroupType;
|
||||||
default_callback_group_ =
|
default_callback_group_ = create_callback_group(
|
||||||
create_callback_group(CallbackGroupType::MutuallyExclusive);
|
CallbackGroupType::MutuallyExclusive);
|
||||||
// TODO(esteve): remove hardcoded values
|
events_publisher_ = create_publisher<rcl_interfaces::msg::ParameterEvent>(
|
||||||
events_publisher_ =
|
"parameter_events", rmw_qos_profile_parameter_events);
|
||||||
create_publisher<rcl_interfaces::msg::ParameterEvent>("parameter_events", 1000);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
rclcpp::callback_group::CallbackGroup::SharedPtr
|
rclcpp::callback_group::CallbackGroup::SharedPtr
|
||||||
|
@ -96,12 +95,13 @@ Node::create_callback_group(
|
||||||
|
|
||||||
template<typename MessageT>
|
template<typename MessageT>
|
||||||
publisher::Publisher::SharedPtr
|
publisher::Publisher::SharedPtr
|
||||||
Node::create_publisher(const std::string & topic_name, size_t queue_size)
|
Node::create_publisher(
|
||||||
|
const std::string & topic_name, const rmw_qos_profile_t & qos_profile)
|
||||||
{
|
{
|
||||||
using rosidl_generator_cpp::get_message_type_support_handle;
|
using rosidl_generator_cpp::get_message_type_support_handle;
|
||||||
auto type_support_handle = get_message_type_support_handle<MessageT>();
|
auto type_support_handle = get_message_type_support_handle<MessageT>();
|
||||||
rmw_publisher_t * publisher_handle = rmw_create_publisher(
|
rmw_publisher_t * publisher_handle = rmw_create_publisher(
|
||||||
node_handle_.get(), type_support_handle, topic_name.c_str(), queue_size);
|
node_handle_.get(), type_support_handle, topic_name.c_str(), qos_profile);
|
||||||
if (!publisher_handle) {
|
if (!publisher_handle) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
|
@ -130,7 +130,7 @@ template<typename MessageT>
|
||||||
typename subscription::Subscription<MessageT>::SharedPtr
|
typename subscription::Subscription<MessageT>::SharedPtr
|
||||||
Node::create_subscription(
|
Node::create_subscription(
|
||||||
const std::string & topic_name,
|
const std::string & topic_name,
|
||||||
size_t queue_size,
|
const rmw_qos_profile_t & qos_profile,
|
||||||
std::function<void(const std::shared_ptr<MessageT> &)> callback,
|
std::function<void(const std::shared_ptr<MessageT> &)> callback,
|
||||||
rclcpp::callback_group::CallbackGroup::SharedPtr group,
|
rclcpp::callback_group::CallbackGroup::SharedPtr group,
|
||||||
bool ignore_local_publications,
|
bool ignore_local_publications,
|
||||||
|
@ -146,7 +146,7 @@ Node::create_subscription(
|
||||||
auto type_support_handle = get_message_type_support_handle<MessageT>();
|
auto type_support_handle = get_message_type_support_handle<MessageT>();
|
||||||
rmw_subscription_t * subscriber_handle = rmw_create_subscription(
|
rmw_subscription_t * subscriber_handle = rmw_create_subscription(
|
||||||
node_handle_.get(), type_support_handle,
|
node_handle_.get(), type_support_handle,
|
||||||
topic_name.c_str(), queue_size, ignore_local_publications);
|
topic_name.c_str(), qos_profile, ignore_local_publications);
|
||||||
if (!subscriber_handle) {
|
if (!subscriber_handle) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
|
|
|
@ -231,9 +231,8 @@ public:
|
||||||
typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
|
typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
|
||||||
on_parameter_event(FunctorT & callback)
|
on_parameter_event(FunctorT & callback)
|
||||||
{
|
{
|
||||||
// TODO(esteve): remove hardcoded values
|
return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
|
||||||
return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>("parameter_events",
|
"parameter_events", rmw_qos_profile_parameter_events, callback);
|
||||||
1000, callback);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue