Skip to content

Commit

Permalink
Merge branch 'master' into qos-4-callbacks
Browse files Browse the repository at this point in the history
  • Loading branch information
@emersonknapp
emersonknapp authored Apr 23, 2019
2 parents edd4007 + afc4cd4 commit 9ef8792
Show file tree
Hide file tree
Showing 6 changed files with 402 additions and 164 deletions.
11 changes: 11 additions & 0 deletions rclpy/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,17 @@ ament_target_dependencies(rclpy_logging
"rcutils"
)

# Signal handling library
add_library(
rclpy_signal_handler
SHARED src/rclpy/_rclpy_signal_handler.c
)
configure_python_c_extension_library(rclpy_signal_handler)
ament_target_dependencies(rclpy_signal_handler
"rcl"
"rcutils"
)

if(NOT WIN32)
ament_environment_hooks(
"${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}"
Expand Down
91 changes: 38 additions & 53 deletions rclpy/rclpy/executors.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
from rclpy.guard_condition import GuardCondition
from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
from rclpy.service import Service
from rclpy.signals import SignalHandlerGuardCondition
from rclpy.subscription import Subscription
from rclpy.task import Future
from rclpy.task import Task
Expand All @@ -46,13 +47,6 @@
from rclpy.waitable import NumberOfEntities
from rclpy.waitable import Waitable

# TODO(wjwwood): make _rclpy_wait(...) thread-safe
# Executor.spin_once() ends up calling _rclpy_wait(...), which right now is
# not thread-safe, no matter if different wait sets are used or not.
# See, for example, https://github.com/ros2/rclpy/issues/192
g_wait_set_spinning_lock = Lock()
g_wait_set_spinning = False

# For documentation purposes
# TODO(jacobperron): Make all entities implement the 'Waitable' interface for better type checking
WaitableEntityType = TypeVar('WaitableEntityType')
Expand Down Expand Up @@ -167,6 +161,7 @@ def __init__(self, *, context: Context = None) -> None:
self._cb_iter = None
self._last_args = None
self._last_kwargs = None
self._sigint_gc = SignalHandlerGuardCondition(context)

@property
def context(self) -> Context:
Expand Down Expand Up @@ -214,6 +209,9 @@ def shutdown(self, timeout_sec: float = None) -> bool:
if self._guard_condition:
_rclpy.rclpy_destroy_entity(self._guard_condition)
self._guard_condition = None
if self._sigint_gc:
self._sigint_gc.destroy()
self._sigint_gc = None
self._cb_iter = None
self._last_args = None
self._last_kwargs = None
Expand All @@ -222,6 +220,8 @@ def shutdown(self, timeout_sec: float = None) -> bool:
def __del__(self):
if self._guard_condition is not None:
_rclpy.rclpy_destroy_entity(self._guard_condition)
if self._sigint_gc is not None:
self._sigint_gc.destroy()

def add_node(self, node: 'Node') -> bool:
"""
Expand Down Expand Up @@ -498,26 +498,23 @@ def _wait_for_ready_callbacks(
)
for waitable in waitables:
waitable.add_to_wait_set(wait_set)
(sigint_gc, sigint_gc_handle) = \
_rclpy.rclpy_get_sigint_guard_condition(self._context.handle)
try:
_rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, sigint_gc)
_rclpy.rclpy_wait_set_add_entity(
'guard_condition', wait_set, self._guard_condition)

# Wait for something to become ready
_rclpy.rclpy_wait(wait_set, timeout_nsec)
if self._is_shutdown:
raise ShutdownException()

# get ready entities
subs_ready = _rclpy.rclpy_get_ready_entities('subscription', wait_set)
guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set)
timers_ready = _rclpy.rclpy_get_ready_entities('timer', wait_set)
clients_ready = _rclpy.rclpy_get_ready_entities('client', wait_set)
services_ready = _rclpy.rclpy_get_ready_entities('service', wait_set)
finally:
_rclpy.rclpy_destroy_entity(sigint_gc)

sigint_gc = self._sigint_gc.guard_handle
_rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, sigint_gc)
_rclpy.rclpy_wait_set_add_entity(
'guard_condition', wait_set, self._guard_condition)

# Wait for something to become ready
_rclpy.rclpy_wait(wait_set, timeout_nsec)
if self._is_shutdown:
raise ShutdownException()

# get ready entities
subs_ready = _rclpy.rclpy_get_ready_entities('subscription', wait_set)
guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set)
timers_ready = _rclpy.rclpy_get_ready_entities('timer', wait_set)
clients_ready = _rclpy.rclpy_get_ready_entities('client', wait_set)
services_ready = _rclpy.rclpy_get_ready_entities('service', wait_set)

# Mark all guards as triggered before yielding since they're auto-taken
for gc in guards:
Expand Down Expand Up @@ -596,33 +593,21 @@ def wait_for_ready_callbacks(self, *args, **kwargs) -> Tuple[Task, WaitableEntit
.. Including the docstring for the hidden function for reference
.. automethod:: _wait_for_ready_callbacks
"""
global g_wait_set_spinning_lock
global g_wait_set_spinning
with g_wait_set_spinning_lock:
if g_wait_set_spinning:
raise RuntimeError(
'Executor.wait_for_ready_callbacks() called concurrently in multiple threads')
g_wait_set_spinning = True

try:
# if an old generator is done, this var makes the loop get a new one before returning
got_generator = False
while not got_generator:
if self._cb_iter is None or self._last_args != args or self._last_kwargs != kwargs:
# Create a new generator
self._last_args = args
self._last_kwargs = kwargs
self._cb_iter = self._wait_for_ready_callbacks(*args, **kwargs)
got_generator = True
# if an old generator is done, this var makes the loop get a new one before returning
got_generator = False
while not got_generator:
if self._cb_iter is None or self._last_args != args or self._last_kwargs != kwargs:
# Create a new generator
self._last_args = args
self._last_kwargs = kwargs
self._cb_iter = self._wait_for_ready_callbacks(*args, **kwargs)
got_generator = True

try:
return next(self._cb_iter)
except StopIteration:
# Generator ran out of work
self._cb_iter = None
finally:
with g_wait_set_spinning_lock:
g_wait_set_spinning = False
try:
return next(self._cb_iter)
except StopIteration:
# Generator ran out of work
self._cb_iter = None


class SingleThreadedExecutor(Executor):
Expand Down
1 change: 1 addition & 0 deletions rclpy/rclpy/impl/implementation_singleton.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,3 +31,4 @@
rclpy_implementation = _import('._rclpy')
rclpy_action_implementation = _import('._rclpy_action')
rclpy_logging_implementation = _import('._rclpy_logging')
rclpy_signal_handler_implementation = _import('._rclpy_signal_handler')
36 changes: 36 additions & 0 deletions rclpy/rclpy/signals.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
# 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 rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
from rclpy.impl.implementation_singleton import rclpy_signal_handler_implementation as _signals
from rclpy.utilities import get_default_context


class SignalHandlerGuardCondition:

def __init__(self, context=None):
if context is None:
context = get_default_context()
self.guard_handle, _ = _rclpy.rclpy_create_guard_condition(context.handle)
_signals.rclpy_register_sigint_guard_condition(self.guard_handle)

def __del__(self):
self.destroy()

def destroy(self):
if self.guard_handle is None:
return
_signals.rclpy_unregister_sigint_guard_condition(self.guard_handle)
_rclpy.rclpy_destroy_entity(self.guard_handle)
self.guard_handle = None
111 changes: 0 additions & 111 deletions rclpy/src/rclpy/_rclpy.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,36 +36,9 @@
#include <rmw/validate_node_name.h>
#include <rosidl_generator_c/message_type_support_struct.h>

#include <signal.h>

#include "rclpy_common/common.h"
#include "./_rclpy_qos_event.c"

static rcl_guard_condition_t * g_sigint_gc_handle;

#ifdef _WIN32
_crt_signal_t g_original_signal_handler = NULL;
#else
sig_t g_original_signal_handler = NULL;
#endif // _WIN32

/// Catch signals
static void catch_function(int signo)
{
(void) signo;
if (NULL != g_sigint_gc_handle) {
rcl_ret_t ret = rcl_trigger_guard_condition(g_sigint_gc_handle);
if (ret != RCL_RET_OK) {
PyErr_Format(PyExc_RuntimeError,
"Failed to trigger guard_condition: %s", rcl_get_error_string().str);
rcl_reset_error();
}
}
if (NULL != g_original_signal_handler) {
g_original_signal_handler(signo);
}
}

void
_rclpy_context_capsule_destructor(PyObject * capsule)
{
Expand Down Expand Up @@ -125,80 +98,6 @@ rclpy_create_context(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args))
return PyCapsule_New(context, "rcl_context_t", _rclpy_context_capsule_destructor);
}

/// Create a sigint guard condition
/**
* A successful call will return a list with two elements:
*
* - a Capsule with the pointer of the created rcl_guard_condition_t * structure
* - an integer representing the memory address of the rcl_guard_condition_t
*
* Raises RuntimeError if initializing the guard condition fails
*
* \return a list with the capsule and memory location, or
* \return NULL on failure
*/
static PyObject *
rclpy_get_sigint_guard_condition(PyObject * Py_UNUSED(self), PyObject * args)
{
PyObject * pycontext;

if (!PyArg_ParseTuple(args, "O", &pycontext)) {
return NULL;
}

rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t");
if (NULL == context) {
return NULL;
}

rcl_guard_condition_t * sigint_gc =
(rcl_guard_condition_t *)PyMem_Malloc(sizeof(rcl_guard_condition_t));
if (!sigint_gc) {
PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for sigint guard condition");
return NULL;
}
*sigint_gc = rcl_get_zero_initialized_guard_condition();
rcl_guard_condition_options_t sigint_gc_options = rcl_guard_condition_get_default_options();

rcl_ret_t ret = rcl_guard_condition_init(sigint_gc, context, sigint_gc_options);
if (ret != RCL_RET_OK) {
PyErr_Format(PyExc_RuntimeError,
"Failed to create guard_condition: %s", rcl_get_error_string().str);
rcl_reset_error();
PyMem_Free(sigint_gc);
return NULL;
}

PyObject * pylist = PyList_New(2);
if (!pylist) {
ret = rcl_guard_condition_fini(sigint_gc);
PyMem_Free(sigint_gc);
return NULL;
}

PyObject * pysigint_gc = PyCapsule_New(sigint_gc, "rcl_guard_condition_t", NULL);
if (!pysigint_gc) {
ret = rcl_guard_condition_fini(sigint_gc);
PyMem_Free(sigint_gc);
Py_DECREF(pylist);
return NULL;
}

PyObject * pysigint_gc_impl_reference = PyLong_FromUnsignedLongLong((uint64_t)&sigint_gc->impl);
if (!pysigint_gc_impl_reference) {
ret = rcl_guard_condition_fini(sigint_gc);
PyMem_Free(sigint_gc);
Py_DECREF(pylist);
Py_DECREF(pysigint_gc);
return NULL;
}

g_sigint_gc_handle = sigint_gc;
PyList_SET_ITEM(pylist, 0, pysigint_gc);
PyList_SET_ITEM(pylist, 1, pysigint_gc_impl_reference);
return pylist;
}

/// Create a general purpose guard condition
/**
* A successful call will return a list with two elements:
Expand Down Expand Up @@ -605,9 +504,6 @@ rclpy_init(PyObject * Py_UNUSED(self), PyObject * args)
}
Py_DECREF(pyseqlist);

// Register our signal handler that will forward to the original one.
g_original_signal_handler = signal(SIGINT, catch_function);

if (PyErr_Occurred()) {
return NULL;
}
Expand Down Expand Up @@ -2406,9 +2302,6 @@ rclpy_destroy_entity(PyObject * Py_UNUSED(self), PyObject * args)
} else if (PyCapsule_IsValid(pyentity, "rcl_guard_condition_t")) {
rcl_guard_condition_t * guard_condition = (rcl_guard_condition_t *)PyCapsule_GetPointer(
pyentity, "rcl_guard_condition_t");
if (g_sigint_gc_handle == guard_condition) {
g_sigint_gc_handle = NULL;
}
ret = rcl_guard_condition_fini(guard_condition);
PyMem_Free(guard_condition);
} else if (PyCapsule_IsValid(pyentity, "rcl_event_t")) {
Expand Down Expand Up @@ -3148,10 +3041,6 @@ rclpy_shutdown(PyObject * Py_UNUSED(self), PyObject * args)
return NULL;
}

// Restore the original signal handler.
signal(SIGINT, g_original_signal_handler);
g_original_signal_handler = NULL;

Py_RETURN_NONE;
}

Expand Down
Loading

0 comments on commit 9ef8792

Please sign in to comment.