Skip to content

Commit

Permalink
Add SubscriptionOptions wrapper
Browse files Browse the repository at this point in the history
Signed-off-by: Clint Lombard <[email protected]>
  • Loading branch information
clintlombard authored and Clint Lombard committed Feb 23, 2023
1 parent 9584697 commit 19ce087
Show file tree
Hide file tree
Showing 9 changed files with 110 additions and 7 deletions.
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
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
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
)
self.subscription_index = None
self.subscription_is_ready = False

Expand Down

0 comments on commit 19ce087

Please sign in to comment.