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.

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

Add SubscriptionOptions wrapper #1074

Open
wants to merge 1 commit into
base: rolling
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
1 change: 1 addition & 0 deletions rclpy/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED
src/rclpy/service_info.cpp
src/rclpy/signal_handler.cpp
src/rclpy/subscription.cpp
src/rclpy/subscription_options.cpp
src/rclpy/time_point.cpp
src/rclpy/timer.cpp
src/rclpy/utils.cpp
Expand Down
11 changes: 9 additions & 2 deletions rclpy/rclpy/node.py
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@
from rclpy.qos_overriding_options import QoSOverridingOptions
from rclpy.service import Service
from rclpy.subscription import Subscription
from rclpy.subscription_options import SubscriptionOptions
from rclpy.time_source import TimeSource
from rclpy.timer import Rate
from rclpy.timer import Timer
Expand Down Expand Up @@ -1508,7 +1509,8 @@ def create_subscription(
callback_group: Optional[CallbackGroup] = None,
event_callbacks: Optional[SubscriptionEventCallbacks] = None,
qos_overriding_options: Optional[QoSOverridingOptions] = None,
raw: bool = False
raw: bool = False,
subscription_options: Optional[SubscriptionOptions] = None,
) -> Subscription:
"""
Create a new subscription.
Expand Down Expand Up @@ -1553,7 +1555,12 @@ def create_subscription(
try:
with self.handle:
subscription_object = _rclpy.Subscription(
self.handle, msg_type, topic, qos_profile.get_c_qos_profile())
self.handle,
msg_type,
topic,
qos_profile.get_c_qos_profile(),
subscription_options,
)
except ValueError:
failed = True
if failed:
Expand Down
18 changes: 18 additions & 0 deletions rclpy/rclpy/subscription_options.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
# Copyright 2023 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy


class SubscriptionOptions(_rclpy.rmw_subscription_options_t):
pass
Comment on lines +17 to +18
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was expecting that abstraction class such as QoSProfile with initialization, setter and getter accessors? so that it would be useful as python client library, and conceal implementation details with C/C++ language.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was trying to keep the implementation more in line with the rclcpp interface, but can look at the changes this might require.

2 changes: 2 additions & 0 deletions rclpy/src/rclpy/_rclpy_pybind11.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#include "service_info.hpp"
#include "signal_handler.hpp"
#include "subscription.hpp"
#include "subscription_options.hpp"
#include "time_point.hpp"
#include "timer.hpp"
#include "utils.hpp"
Expand Down Expand Up @@ -144,6 +145,7 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
rclpy::define_guard_condition(m);
rclpy::define_timer(m);
rclpy::define_subscription(m);
rclpy::define_subscription_options(m);
rclpy::define_time_point(m);
rclpy::define_clock(m);
rclpy::define_waitset(m);
Expand Down
10 changes: 8 additions & 2 deletions rclpy/src/rclpy/subscription.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <rcl/types.h>
#include <rosidl_runtime_c/message_type_support_struct.h>
#include <rmw/types.h>
#include <rmw/subscription_options.h>

#include <memory>
#include <stdexcept>
Expand All @@ -37,7 +38,7 @@ namespace rclpy
{
Subscription::Subscription(
Node & node, py::object pymsg_type, std::string topic,
py::object pyqos_profile)
py::object pyqos_profile, py::object pysubscription_options)
: node_(node)
{
auto msg_type = static_cast<rosidl_message_type_support_t *>(
Expand All @@ -52,6 +53,11 @@ Subscription::Subscription(
subscription_ops.qos = pyqos_profile.cast<rmw_qos_profile_t>();
}

if (!pysubscription_options.is_none()) {
subscription_ops.rmw_subscription_options =
pysubscription_options.cast<rmw_subscription_options_t>();
}

rcl_subscription_ = std::shared_ptr<rcl_subscription_t>(
new rcl_subscription_t,
[node](rcl_subscription_t * subscription)
Expand Down Expand Up @@ -172,7 +178,7 @@ void
define_subscription(py::object module)
{
py::class_<Subscription, Destroyable, std::shared_ptr<Subscription>>(module, "Subscription")
.def(py::init<Node &, py::object, std::string, py::object>())
.def(py::init<Node &, py::object, std::string, py::object, py::object>())
.def_property_readonly(
"pointer", [](const Subscription & subscription) {
return reinterpret_cast<size_t>(subscription.rcl_ptr());
Expand Down
5 changes: 3 additions & 2 deletions rclpy/src/rclpy/subscription.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,10 +46,11 @@ class Subscription : public Destroyable, public std::enable_shared_from_this<Sub
* \param[in] pymsg_type Message module associated with the subscriber
* \param[in] topic The topic name
* \param[in] pyqos_profile rmw_qos_profile_t object for this subscription
* \param[in] pysubscription_options rmw_subscription_options_t object for this subscription
*/
Subscription(
Node & node, py::object pymsg_type, std::string topic,
py::object pyqos_profile);
py::object pyqos_profile, py::object pysubscription_options);

/// Take a message and its metadata from a subscription
/**
Expand Down Expand Up @@ -100,7 +101,7 @@ class Subscription : public Destroyable, public std::enable_shared_from_this<Sub
Node node_;
std::shared_ptr<rcl_subscription_t> rcl_subscription_;
};
/// Define a pybind11 wrapper for an rclpy::Service
/// Define a pybind11 wrapper for an rclpy::Subscription
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good eye 👍

void define_subscription(py::object module);
} // namespace rclpy

Expand Down
35 changes: 35 additions & 0 deletions rclpy/src/rclpy/subscription_options.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
// Copyright 2023 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <pybind11/pybind11.h>

#include <rmw/types.h>
#include <rmw/subscription_options.h>

#include "subscription_options.hpp"

using pybind11::literals::operator""_a;

namespace rclpy
{
void
define_subscription_options(py::object module)
{
py::class_<rmw_subscription_options_t>(module, "rmw_subscription_options_t")
.def(py::init<>(&rmw_get_default_subscription_options))
.def_readwrite(
"ignore_local_publications",
&rmw_subscription_options_t::ignore_local_publications);
}
} // namespace rclpy
28 changes: 28 additions & 0 deletions rclpy/src/rclpy/subscription_options.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
// Copyright 2023 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLPY__SUBSCRIPTION_OPTIONS_HPP_
#define RCLPY__SUBSCRIPTION_OPTIONS_HPP_

#include <pybind11/pybind11.h>

namespace py = pybind11;

namespace rclpy
{
/// Define a pybind11 wrapper for an rclpy::SubscriptionOptions
void define_subscription_options(py::object module);
} // namespace rclpy

#endif // RCLPY__SUBSCRIPTION_OPTIONS_HPP_
7 changes: 6 additions & 1 deletion rclpy/test/test_waitable.py
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,12 @@ def __init__(self, node):

with node.handle:
self.subscription = _rclpy.Subscription(
node.handle, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile())
node.handle,
EmptyMsg,
'test_topic',
QoSProfile(depth=10).get_c_qos_profile(),
None
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe that we need to add more test cases here,

  • actually use SubscriptionOptions as argument. (Not None case)
  • what is the default value for SubscriptionOptions?
  • verify SubscriptionOptions member variables set by user application?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good idea. I'll add a test case for this.

)
self.subscription_index = None
self.subscription_is_ready = False

Expand Down