forked from ros2/rclpy
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Emerson Knapp <eknapp@amazon.com>
- Loading branch information
Emerson Knapp
committed
Apr 16, 2019
1 parent
4802211
commit ae0f5eb
Showing
9 changed files
with
602 additions
and
18 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,172 @@ | ||
# Copyright 2019 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 collections import namedtuple | ||
from enum import IntEnum | ||
from typing import Callable | ||
|
||
from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy | ||
from rclpy.waitable import NumberOfEntities | ||
from rclpy.waitable import Waitable | ||
|
||
|
||
class QoSPublisherEventType(IntEnum): | ||
""" | ||
Enum for types of QoS events that a Publisher can receive. | ||
This enum matches the one defined in rcl/event.h | ||
""" | ||
|
||
RCL_PUBLISHER_OFFERED_DEADLINE_MISSED = 0 | ||
RCL_PUBLISHER_LIVELINESS_LOST = 1 | ||
|
||
|
||
class QoSSubscriptionEventType(IntEnum): | ||
""" | ||
Enum for types of QoS events that a Subscription can receive. | ||
This enum matches the one defined in rcl/event.h | ||
""" | ||
|
||
RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED = 0 | ||
RCL_SUBSCRIPTION_LIVELINESS_CHANGED = 1 | ||
|
||
|
||
""" | ||
Payload type for Subscription Deadline callback. | ||
Mirrors rmw_requested_deadline_missed_status_t from rmw/types.h | ||
""" | ||
QoSRequestedDeadlineMissedInfo = namedtuple( | ||
'QoSRequestedDeadlineMissedInfo', | ||
('total_count', 'total_count_change')) | ||
|
||
""" | ||
Payload type for Subscription Liveliness callback. | ||
Mirrors rmw_liveliness_changed_status_t from rmw/types.h | ||
""" | ||
QoSLivelinessChangedInfo = namedtuple( | ||
'QoSLivelinessChangedInfo', | ||
('alive_count', 'not_alive_count', 'alive_count_change', 'not_alive_count_change')) | ||
|
||
""" | ||
Payload type for Publisher Deadline callback. | ||
Mirrors rmw_offered_deadline_missed_status_t from rmw/types.h | ||
""" | ||
QoSOfferedDeadlineMissedInfo = namedtuple( | ||
'QoSOfferedDeadlineMissedInfo', | ||
('total_count', 'total_count_change')) | ||
|
||
""" | ||
Payload type for Publisher Liveliness callback. | ||
Mirrors rmw_liveliness_lost_status_t from rmw/types.h | ||
""" | ||
QoSLivelinessLostInfo = namedtuple( | ||
'QoSLivelinessLostInfo', | ||
('total_count', 'total_count_change')) | ||
|
||
|
||
class SubscriptionEventCallbacks: | ||
"""Container to provide middleware event callbacks for a Subscription.""" | ||
|
||
def __init__( | ||
self, | ||
*, | ||
deadline: Callable[[QoSRequestedDeadlineMissedInfo], None] = None, | ||
liveliness: Callable[[QoSLivelinessChangedInfo], None] = None, | ||
) -> None: | ||
""" | ||
Constructor. | ||
:param deadline: A user-defined callback that is called when a topic misses our | ||
requested Deadline. | ||
:param liveliness: A user-defined callback that is called when the Liveliness of | ||
a Publisher on subscribed topic changes. | ||
""" | ||
self.deadline = deadline | ||
self.liveliness = liveliness | ||
|
||
|
||
class PublisherEventCallbacks: | ||
"""Container to provide middleware event callbacks for a Publisher.""" | ||
|
||
def __init__( | ||
self, | ||
*, | ||
deadline: Callable[[QoSOfferedDeadlineMissedInfo], None] = None, | ||
liveliness: Callable[[QoSLivelinessLostInfo], None] = None | ||
) -> None: | ||
""" | ||
Constructor. | ||
:param deadline: A user-defined callback that is called when the Publisher misses | ||
its offered Deadline. | ||
:param liveliness: A user-defined callback that is called when this Publisher | ||
fails to signal its Liveliness and is reported as not-alive. | ||
""" | ||
self.deadline = deadline | ||
self.liveliness = liveliness | ||
|
||
|
||
class QoSEventHandler(Waitable): | ||
"""Waitable type to handle QoS events.""" | ||
|
||
def __init__( | ||
self, | ||
*, | ||
callback_group, | ||
callback, | ||
event_type, | ||
parent_handle, | ||
): | ||
# Waitable init adds self to callback_group | ||
super().__init__(callback_group) | ||
self.event_type = event_type | ||
self.callback = callback | ||
|
||
self._parent_handle = parent_handle | ||
self._event_handle = _rclpy.rclpy_create_event(event_type, parent_handle) | ||
self._event_is_ready = False | ||
self._event_index = None | ||
|
||
# Start Waitable API | ||
def is_ready(self, wait_set): | ||
"""Return True if entities are ready in the wait set.""" | ||
if _rclpy.rclpy_wait_set_is_ready('event', wait_set, self._event_index): | ||
self._event_is_ready = True | ||
return self._event_is_ready | ||
|
||
def take_data(self): | ||
"""Take stuff from lower level so the wait set doesn't immediately wake again.""" | ||
if self._event_is_ready: | ||
self._event_is_ready = False | ||
return _rclpy.rclpy_take_event( | ||
self._event_handle, self._parent_handle, self.event_type) | ||
return None | ||
|
||
async def execute(self, taken_data): | ||
"""Execute work after data has been taken from a ready wait set.""" | ||
self.callback(taken_data) | ||
|
||
def get_num_entities(self): | ||
"""Return number of each type of entity used.""" | ||
return NumberOfEntities(num_events=1) | ||
|
||
def add_to_wait_set(self, wait_set): | ||
"""Add entites to wait set.""" | ||
self._event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, self._event_handle) | ||
# End Waitable API |
Oops, something went wrong.