From 160438ddc38dab4c4a19dd6b1b02b541ddc0a503 Mon Sep 17 00:00:00 2001 From: Andrew Messing Date: Wed, 6 Dec 2023 15:27:45 +0000 Subject: [PATCH] Removed top level pyproject.toml --- .pre-commit-config.yaml | 5 +- .../bdai_ros2_wrappers/action_client.py | 22 +++- .../bdai_ros2_wrappers/action_handle.py | 26 +++- .../bdai_ros2_wrappers/context.py | 4 +- .../bdai_ros2_wrappers/executors.py | 122 +++++++++++++----- .../bdai_ros2_wrappers/futures.py | 7 +- .../bdai_ros2_wrappers/logging.py | 11 +- bdai_ros2_wrappers/bdai_ros2_wrappers/node.py | 7 +- .../bdai_ros2_wrappers/process.py | 38 ++++-- .../bdai_ros2_wrappers/scope.py | 67 +++++++--- .../bdai_ros2_wrappers/service_handle.py | 15 ++- .../single_goal_action_server.py | 12 +- .../single_goal_multiple_action_servers.py | 13 +- .../static_transform_broadcaster.py | 14 +- .../bdai_ros2_wrappers/subscription.py | 8 +- .../bdai_ros2_wrappers/tf_listener_wrapper.py | 27 +++- .../bdai_ros2_wrappers/utilities.py | 4 +- .../examples/background_action_example.py | 8 +- .../examples/talker_listener_example.py | 8 +- bdai_ros2_wrappers/examples/tf_cli_example.py | 13 +- bdai_ros2_wrappers/package.xml | 2 +- bdai_ros2_wrappers/setup.py | 6 +- bdai_ros2_wrappers/test/test_action_client.py | 12 +- bdai_ros2_wrappers/test/test_action_handle.py | 34 +++-- bdai_ros2_wrappers/test/test_executors.py | 23 +++- bdai_ros2_wrappers/test/test_integration.py | 35 +++-- bdai_ros2_wrappers/test/test_process.py | 12 +- .../test/test_service_handle.py | 4 +- .../test/test_single_goal_action_server.py | 4 +- ...est_single_goal_multiple_action_servers.py | 29 ++++- .../test/test_static_transform_broadcaster.py | 12 +- .../test/test_tf_listener_wrapper.py | 56 ++++++-- configs/black.toml | 13 ++ configs/mypy.ini | 15 +++ configs/ruff.toml | 49 +++++++ pyproject.toml | 98 -------------- 36 files changed, 581 insertions(+), 254 deletions(-) create mode 100644 configs/black.toml create mode 100644 configs/mypy.ini create mode 100644 configs/ruff.toml delete mode 100644 pyproject.toml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fa1aef3..2a2ce72 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -5,13 +5,13 @@ repos: rev: 'v0.0.263' hooks: - id: ruff - args: ['--fix', '--config', 'pyproject.toml'] + args: ['--fix', '--config', 'configs/ruff.toml'] - repo: https://github.com/psf/black rev: 23.3.0 hooks: - id: black language_version: python3.10 - args: ['--config', 'pyproject.toml'] + args: ['--config', 'configs/black.toml'] verbose: true - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.4.0 @@ -32,6 +32,7 @@ repos: rev: v1.2.0 hooks: - id: mypy + args: ['--config-file', 'configs/mypy.ini'] pass_filenames: false additional_dependencies: - types-protobuf diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/action_client.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/action_client.py index 0ae6214..781343a 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/action_client.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/action_client.py @@ -12,7 +12,9 @@ class ActionClientWrapper(rclpy.action.ActionClient): """A wrapper for ros2's ActionClient for extra functionality""" - def __init__(self, action_type: Type[Action], action_name: str, node: Optional[Node] = None) -> None: + def __init__( + self, action_type: Type[Action], action_name: str, node: Optional[Node] = None + ) -> None: """Constructor Args: @@ -22,7 +24,9 @@ def __init__(self, action_type: Type[Action], action_name: str, node: Optional[N """ node = node or scope.node() if node is None: - raise ValueError("no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)") + raise ValueError( + "no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)" + ) self._node = node super().__init__(self._node, action_type, action_name) self._node.get_logger().info(f"Waiting for action server for {action_name}") @@ -60,7 +64,9 @@ def _on_failure() -> None: nonlocal failed failed = True - handle = self.send_goal_async_handle(action_name=action_name, goal=goal, on_failure_callback=_on_failure) + handle = self.send_goal_async_handle( + action_name=action_name, goal=goal, on_failure_callback=_on_failure + ) handle.set_on_cancel_success_callback(_on_cancel_succeeded) if not handle.wait_for_result(timeout_sec=timeout_sec): # If the action didn't fail and wasn't canceled then it timed out and should be canceled @@ -96,7 +102,11 @@ def send_goal_async_handle( Returns: ActionHandle: An object to manage the asynchronous lifecycle of the action request """ - handle = ActionHandle(action_name=action_name, logger=self._node.get_logger(), context=self._node.context) + handle = ActionHandle( + action_name=action_name, + logger=self._node.get_logger(), + context=self._node.context, + ) if result_callback is not None: handle.set_result_callback(result_callback) @@ -107,7 +117,9 @@ def send_goal_async_handle( send_goal_future = self.send_goal_async(goal) else: handle.set_feedback_callback(feedback_callback) - send_goal_future = self.send_goal_async(goal, feedback_callback=handle.get_feedback_callback) + send_goal_future = self.send_goal_async( + goal, feedback_callback=handle.get_feedback_callback + ) handle.set_send_goal_future(send_goal_future) return handle diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/action_handle.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/action_handle.py index da299c2..5560e50 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/action_handle.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/action_handle.py @@ -17,7 +17,12 @@ class ActionHandle(object): as holding the various callbacks for sending an ActionGoal (cancel, failure, feedback, result) """ - def __init__(self, action_name: str, logger: Optional[RcutilsLogger] = None, context: Optional[Context] = None): + def __init__( + self, + action_name: str, + logger: Optional[RcutilsLogger] = None, + context: Optional[Context] = None, + ): """Constructor Args: @@ -68,7 +73,8 @@ def wait_for_result(self, timeout_sec: Optional[float] = None) -> bool: True if successful; False if the timeout was triggered or the action was rejected, cancelled, or abort """ return self._wait_for_result_event.wait(timeout=timeout_sec) and ( - self._result is not None and self._result.status == GoalStatus.STATUS_SUCCEEDED + self._result is not None + and self._result.status == GoalStatus.STATUS_SUCCEEDED ) def wait_for_acceptance(self, timeout_sec: Optional[float] = None) -> bool: @@ -91,11 +97,15 @@ def set_send_goal_future(self, send_goal_future: Future) -> None: self._send_goal_future = send_goal_future self._send_goal_future.add_done_callback(self._goal_response_callback) - def set_feedback_callback(self, feedback_callback: Callable[[Action.Feedback], None]) -> None: + def set_feedback_callback( + self, feedback_callback: Callable[[Action.Feedback], None] + ) -> None: """Sets the callback used to process feedback received while an Action is being executed""" self._feedback_callback = feedback_callback - def set_result_callback(self, result_callback: Callable[[Action.Result], None]) -> None: + def set_result_callback( + self, result_callback: Callable[[Action.Result], None] + ) -> None: """Sets the callback for processing the result from executing an Action""" self._result_callback = result_callback @@ -103,11 +113,15 @@ def set_on_failure_callback(self, on_failure_callback: Callable) -> None: """Set the callback to execute upon failure""" self._on_failure_callback = on_failure_callback - def set_on_cancel_success_callback(self, on_cancel_success_callback: Callable) -> None: + def set_on_cancel_success_callback( + self, on_cancel_success_callback: Callable + ) -> None: """Set the callback to execute upon successfully canceling the action""" self._on_cancel_success_callback = on_cancel_success_callback - def set_on_cancel_failure_callback(self, on_cancel_failure_callback: Callable) -> None: + def set_on_cancel_failure_callback( + self, on_cancel_failure_callback: Callable + ) -> None: """Set the callback to execute upon failing to cancel the action""" self._on_cancel_failure_callback = on_cancel_failure_callback diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/context.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/context.py index b47a04e..9ef7b81 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/context.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/context.py @@ -6,7 +6,9 @@ from rclpy.utilities import get_default_context -def wait_for_shutdown(*, timeout_sec: Optional[float] = None, context: Optional[Context] = None) -> bool: +def wait_for_shutdown( + *, timeout_sec: Optional[float] = None, context: Optional[Context] = None +) -> bool: """ Wait for context shutdown. diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/executors.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/executors.py index 95b5ceb..95fc65b 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/executors.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/executors.py @@ -108,7 +108,9 @@ def __str__(self) -> str: class Worker(threading.Thread): """A worker in its own daemonized OS thread.""" - def __init__(self, executor_weakref: weakref.ref, stop_on_timeout: bool = True) -> None: + def __init__( + self, executor_weakref: weakref.ref, stop_on_timeout: bool = True + ) -> None: """ Initializes the worker. @@ -137,18 +139,24 @@ def run(self) -> None: work: typing.Optional[AutoScalingThreadPool.Work] = None while True: if AutoScalingThreadPool._interpreter_shutdown: - self._logger.debug("Interpreter is shutting down! Terminating worker...") + self._logger.debug( + "Interpreter is shutting down! Terminating worker..." + ) self._runqueue.put(None) break - executor: typing.Optional[AutoScalingThreadPool] = self._executor_weakref() + executor: typing.Optional[ + AutoScalingThreadPool + ] = self._executor_weakref() if executor is None: self._logger.debug("Executor is gone! Terminating worker...") self._runqueue.put(None) break if executor._shutdown: - self._logger.debug("Executor is shutting down! Terminating worker...") + self._logger.debug( + "Executor is shutting down! Terminating worker..." + ) self._runqueue.put(None) break @@ -241,7 +249,9 @@ def __init__( if max_workers <= 0: raise ValueError("Maximum number of workers must be a positive number") if max_workers < min_workers: - raise ValueError("Maximum number of workers must be larger than or equal to the minimum number of workers") + raise ValueError( + "Maximum number of workers must be larger than or equal to the minimum number of workers" + ) self._max_workers = max_workers if max_idle_time is None: @@ -272,12 +282,12 @@ def __init__( self._shutdown_lock = threading.Lock() self._scaling_event = threading.Condition() - self._waitqueues: typing.Dict[typing.Callable[..., typing.Any], collections.deque] = collections.defaultdict( - collections.deque - ) - self._runlists: typing.Dict[typing.Callable[..., typing.Any], typing.List[AutoScalingThreadPool.Work]] = ( - collections.defaultdict(list) - ) + self._waitqueues: typing.Dict[ + typing.Callable[..., typing.Any], collections.deque + ] = collections.defaultdict(collections.deque) + self._runlists: typing.Dict[ + typing.Callable[..., typing.Any], typing.List[AutoScalingThreadPool.Work] + ] = collections.defaultdict(list) self._runslots = threading.Semaphore(0) runqueue: queue.SimpleQueue = queue.SimpleQueue() @@ -285,20 +295,32 @@ def __init__( with AutoScalingThreadPool._lock: if AutoScalingThreadPool._interpreter_shutdown: - raise RuntimeError("cannot create thread pool while interpreter is shutting down") + raise RuntimeError( + "cannot create thread pool while interpreter is shutting down" + ) self._runqueue = runqueue - self._logger.debug("Registering runqueue for external wake up on interpreter shutdown...") + self._logger.debug( + "Registering runqueue for external wake up on interpreter shutdown..." + ) AutoScalingThreadPool._all_runqueues.add(runqueue) self._logger.debug("Done registering runqueue") - self._workers: weakref.WeakSet[AutoScalingThreadPool.Worker] = weakref.WeakSet() + self._workers: weakref.WeakSet[ + AutoScalingThreadPool.Worker + ] = weakref.WeakSet() if self._min_workers > 0: with self._scaling_event: - self._logger.debug(f"Pre-populating pool with {self._min_workers} workers") + self._logger.debug( + f"Pre-populating pool with {self._min_workers} workers" + ) for _ in range(self._min_workers): # fire up stable worker pool - worker = AutoScalingThreadPool.Worker(self._weak_self, stop_on_timeout=False) + worker = AutoScalingThreadPool.Worker( + self._weak_self, stop_on_timeout=False + ) # register worker for external joining on interpreter shutdown - self._logger.debug("Registering worker for external joining on interpreter shutdown...") + self._logger.debug( + "Registering worker for external joining on interpreter shutdown..." + ) AutoScalingThreadPool._all_workers.add(worker) self._logger.debug("Done registering worker") self._logger.debug("Adding worker to the pool...") @@ -323,15 +345,25 @@ def scaling_event(self) -> threading.Condition: def working(self) -> bool: """Whether work is ongoing or not.""" with self._submit_lock: - return any(work.pending() for runlist in self._runlists.values() for work in runlist) or any( - work.pending() for waitqueue in self._waitqueues.values() for work in waitqueue + return any( + work.pending() + for runlist in self._runlists.values() + for work in runlist + ) or any( + work.pending() + for waitqueue in self._waitqueues.values() + for work in waitqueue ) @property def capped(self) -> bool: """Whether submission quotas are in force or not.""" with self._submit_lock: - return any(work.pending() for waitqueue in self._waitqueues.values() for work in waitqueue) + return any( + work.pending() + for waitqueue in self._waitqueues.values() + for work in waitqueue + ) def wait(self, timeout: typing.Optional[float] = None) -> bool: """ @@ -347,8 +379,14 @@ def wait(self, timeout: typing.Optional[float] = None) -> bool: True if all work completed, False if the wait timed out. """ with self._submit_lock: - futures = [work.future for runlist in self._runlists.values() for work in runlist] - futures += [work.future for waitqueue in self._waitqueues.values() for work in waitqueue] + futures = [ + work.future for runlist in self._runlists.values() for work in runlist + ] + futures += [ + work.future + for waitqueue in self._waitqueues.values() + for work in waitqueue + ] done, not_done = concurrent.futures.wait(futures, timeout=timeout) return len(not_done) == 0 @@ -357,7 +395,9 @@ def _cleanup_after(self, work: "AutoScalingThreadPool.Work") -> bool: with self._submit_lock: self._logger.debug(f"Cleaning up after work '{work}'") self._runlists[work.fn].remove(work) - if work.fn in self._waitqueues and self._waitqueues[work.fn]: # continue with pending work + if ( + work.fn in self._waitqueues and self._waitqueues[work.fn] + ): # continue with pending work self._logger.debug("Have similar work pending!") self._logger.debug("Fetching pending work...") work = self._waitqueues[work.fn].popleft() @@ -403,7 +443,11 @@ def _do_submit(self, work: "AutoScalingThreadPool.Work") -> None: # NOTE(hidmic): cannot recreate type signature for method override # See https://github.com/python/typeshed/blob/main/stdlib/concurrent/futures/_base.pyi. def submit( # type: ignore - self, fn: typing.Callable[..., typing.Any], /, *args: typing.Any, **kwargs: typing.Any + self, + fn: typing.Callable[..., typing.Any], + /, + *args: typing.Any, + **kwargs: typing.Any, ) -> concurrent.futures.Future: """ Submits work to the pool. @@ -429,7 +473,9 @@ def submit( # type: ignore f"Submitting work '{work}'...", ) if self._submission_quota > len(self._runlists[work.fn]): - if work.fn in self._waitqueues and self._waitqueues[work.fn]: # prioritize pending work + if ( + work.fn in self._waitqueues and self._waitqueues[work.fn] + ): # prioritize pending work self._logger.debug("Have similar work pending") self._logger.debug(f"Work '{work}' put to wait", work) self._waitqueues[work.fn].append(work) @@ -513,7 +559,11 @@ class AutoScalingMultiThreadedExecutor(rclpy.executors.Executor): class Task: """A bundle of an executable task and its associated entity.""" - def __init__(self, task: rclpy.task.Task, entity: typing.Optional[rclpy.executors.WaitableEntityType]) -> None: + def __init__( + self, + task: rclpy.task.Task, + entity: typing.Optional[rclpy.executors.WaitableEntityType], + ) -> None: self.task = task self.entity = entity self.callback_group = entity.callback_group if entity is not None else None @@ -577,7 +627,9 @@ def __init__( submission_quota=max_threads_per_callback_group, logger=logger, ) - self._wip: typing.Dict[AutoScalingMultiThreadedExecutor.Task, concurrent.futures.Future] = {} + self._wip: typing.Dict[ + AutoScalingMultiThreadedExecutor.Task, concurrent.futures.Future + ] = {} @property def thread_pool(self) -> AutoScalingThreadPool: @@ -605,7 +657,9 @@ def _do_spin_once(self, *args: typing.Any, **kwargs: typing.Any) -> None: # submission future is done. That is, a task could be legitimately ready for # dispatch and be missed. Fortunately, this will only delay dispatch until the # next spin cycle. - if task not in self._wip or (self._wip[task].done() and not task.done()): + if task not in self._wip or ( + self._wip[task].done() and not task.done() + ): self._wip[task] = self._thread_pool.submit(task) for task in list(self._wip): if task.done(): @@ -655,7 +709,9 @@ def shutdown(self, timeout_sec: typing.Optional[float] = None) -> bool: @contextlib.contextmanager -def background(executor: rclpy.executors.Executor) -> typing.Iterator[rclpy.executors.Executor]: +def background( + executor: rclpy.executors.Executor, +) -> typing.Iterator[rclpy.executors.Executor]: """ Pushes an executor to a background thread. @@ -671,7 +727,9 @@ def background(executor: rclpy.executors.Executor) -> typing.Iterator[rclpy.exec background_thread = threading.Thread(target=executor.spin) executor.spin = bind_to_thread(executor.spin, background_thread) executor.spin_once = bind_to_thread(executor.spin_once, background_thread) - executor.spin_until_future_complete = bind_to_thread(executor.spin_until_future_complete, background_thread) + executor.spin_until_future_complete = bind_to_thread( + executor.spin_until_future_complete, background_thread + ) executor.spin_once_until_future_complete = bind_to_thread( executor.spin_once_until_future_complete, background_thread ) @@ -684,7 +742,9 @@ def background(executor: rclpy.executors.Executor) -> typing.Iterator[rclpy.exec @contextlib.contextmanager -def foreground(executor: rclpy.executors.Executor) -> typing.Iterator[rclpy.executors.Executor]: +def foreground( + executor: rclpy.executors.Executor, +) -> typing.Iterator[rclpy.executors.Executor]: """ Manages an executor in the current thread. diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/futures.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/futures.py index 02c7805..cdcd235 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/futures.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/futures.py @@ -7,7 +7,12 @@ from rclpy.utilities import get_default_context -def wait_for_future(future: Future, timeout_sec: Optional[float] = None, *, context: Optional[Context] = None) -> bool: +def wait_for_future( + future: Future, + timeout_sec: Optional[float] = None, + *, + context: Optional[Context] = None +) -> bool: """Blocks while waiting for a future to become done Args: diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/logging.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/logging.py index 5af1073..b36282a 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/logging.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/logging.py @@ -42,7 +42,9 @@ class RcutilsLogHandler(logging.Handler): default_formatter = logging.Formatter("(logging.%(name)s) %(message)s") - def __init__(self, node: Node, level: typing.Union[int, str] = logging.NOTSET) -> None: + def __init__( + self, node: Node, level: typing.Union[int, str] = logging.NOTSET + ) -> None: super().__init__(level=level) self.node = node self.logger = node.get_logger() @@ -61,7 +63,12 @@ def emit(self, record: logging.LogRecord) -> None: # NOTE(hidmic): this bypasses the rclpy logger API to avoid the extra meaningless # computations (e.g. call stack inspection). impl.rclpy_logging_rcutils_log( - severity, self.logger.name, message, record.funcName, record.pathname, record.lineno + severity, + self.logger.name, + message, + record.funcName, + record.pathname, + record.lineno, ) except Exception: self.handleError(record) diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/node.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/node.py index 7a5c0ca..9061911 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/node.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/node.py @@ -14,7 +14,12 @@ class Node(BaseNode): * changes the default callback group to be non-reentrant. """ - def __init__(self, *args: Any, default_callback_group: Optional[CallbackGroup] = None, **kwargs: Any) -> None: + def __init__( + self, + *args: Any, + default_callback_group: Optional[CallbackGroup] = None, + **kwargs: Any + ) -> None: """ Initializes the node. diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/process.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/process.py index 7e6316f..ddeaec6 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/process.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/process.py @@ -25,7 +25,9 @@ MainCallableTakingArgs = typing.Callable[[argparse.Namespace], typing.Optional[int]] MainCallableTakingArgv = typing.Callable[[typing.Sequence[str]], typing.Optional[int]] MainCallableTakingNoArgs = typing.Callable[[], typing.Optional[int]] -MainCallable = typing.Union[MainCallableTakingNoArgs, MainCallableTakingArgv, MainCallableTakingArgs] +MainCallable = typing.Union[ + MainCallableTakingNoArgs, MainCallableTakingArgv, MainCallableTakingArgs +] class ROSAwareProcess: @@ -90,13 +92,17 @@ def __init__( validate_node_name(name) prebaked = name except InvalidNodeNameException: - warnings.warn(f"'{name}' cannot be used as node name, using scope default") + warnings.warn( + f"'{name}' cannot be used as node name, using scope default" + ) if namespace is True: try: validate_namespace(name) namespace = name except InvalidNamespaceException: - warnings.warn(f"'{name}' cannot be used as namespace, using scope default") + warnings.warn( + f"'{name}' cannot be used as namespace, using scope default" + ) if forward_logging is None: forward_logging = bool(prebaked) self._func = func @@ -138,7 +144,9 @@ def __setattr__(self, name: str, value: typing.Any) -> None: return super().__setattr__(name, value) - def __call__(self, argv: typing.Optional[typing.Sequence[str]] = None) -> typing.Optional[int]: + def __call__( + self, argv: typing.Optional[typing.Sequence[str]] = None + ) -> typing.Optional[int]: """ Invokes wrapped process function in a ROS 2 aware scope. @@ -172,10 +180,14 @@ def __call__(self, argv: typing.Optional[typing.Sequence[str]] = None) -> typing try: sig = inspect.signature(self._func) if not sig.parameters: - func_taking_no_args = typing.cast(MainCallableTakingNoArgs, self._func) + func_taking_no_args = typing.cast( + MainCallableTakingNoArgs, self._func + ) return func_taking_no_args() if args is not None: - func_taking_args = typing.cast(MainCallableTakingArgs, self._func) + func_taking_args = typing.cast( + MainCallableTakingArgs, self._func + ) return func_taking_args(args) func_taking_argv = typing.cast(MainCallableTakingArgv, self._func) return func_taking_argv(rclpy.utilities.remove_ros_args(argv)) @@ -202,7 +214,9 @@ def wait_for_shutdown(self, *, timeout_sec: typing.Optional[float] = None) -> bo with self._lock: if self._scope is None: raise RuntimeError("process is not executing") - return context.wait_for_shutdown(timeout_sec=timeout_sec, context=self._scope.context) + return context.wait_for_shutdown( + timeout_sec=timeout_sec, context=self._scope.context + ) def try_shutdown(self) -> None: """Atempts to shutdown the underlying scope context.""" @@ -241,7 +255,9 @@ def tf_listener() -> typing.Optional[TFListenerWrapper]: return process.tf_listener -def load(factory: AnyEntityFactoryCallable, *args: typing.Any, **kwargs: typing.Any) -> AnyEntity: +def load( + factory: AnyEntityFactoryCallable, *args: typing.Any, **kwargs: typing.Any +) -> AnyEntity: """ Loads a ROS 2 node (or a collection thereof) within the current ROS 2 aware process scope. @@ -291,7 +307,11 @@ def managed( return process.managed(factory, *args, **kwargs) -def spin(factory: typing.Optional[AnyEntityFactoryCallable] = None, *args: typing.Any, **kwargs: typing.Any) -> None: +def spin( + factory: typing.Optional[AnyEntityFactoryCallable] = None, + *args: typing.Any, + **kwargs: typing.Any, +) -> None: """ Spins current ROS 2 aware process executor (and all ROS 2 nodes in it). diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py index 47775ad..592a33c 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py @@ -9,7 +9,11 @@ import rclpy.logging import rclpy.node -from bdai_ros2_wrappers.executors import AutoScalingMultiThreadedExecutor, background, foreground +from bdai_ros2_wrappers.executors import ( + AutoScalingMultiThreadedExecutor, + background, + foreground, +) from bdai_ros2_wrappers.logging import logs_to_ros from bdai_ros2_wrappers.node import Node from bdai_ros2_wrappers.tf_listener_wrapper import TFListenerWrapper @@ -110,25 +114,35 @@ def __enter__(self) -> "ROSAwareScope": if self._global: if threading.current_thread() is not threading.main_thread(): - raise RuntimeError("a global scope can only be entered from main thread") + raise RuntimeError( + "a global scope can only be entered from main thread" + ) if ROSAwareScope.global_ is not None: raise RuntimeError("a global scope has been entered already") if ROSAwareScope.local.top is not None: - raise RuntimeError("a global scope must be entered before any local scopes") + raise RuntimeError( + "a global scope must be entered before any local scopes" + ) self._stack = contextlib.ExitStack() self._owner = threading.current_thread() if self._prebaked or self._autospin: - logger = rclpy.logging.get_logger(self._namespace or fqn(self.__class__)) - executor = AutoScalingMultiThreadedExecutor(logger=logger, context=self._context) + logger = rclpy.logging.get_logger( + self._namespace or fqn(self.__class__) + ) + executor = AutoScalingMultiThreadedExecutor( + logger=logger, context=self._context + ) if self._autospin: self._executor = self._stack.enter_context(background(executor)) else: self._executor = self._stack.enter_context(foreground(executor)) if self._prebaked: - node = Node(self._prebaked, namespace=self._namespace, context=self._context) + node = Node( + self._prebaked, namespace=self._namespace, context=self._context + ) self._executor.add_node(node) self._graph.append(node) if self._uses_tf: @@ -303,7 +317,9 @@ def managed( self.unload(loaded) @typing.overload - def load(self, factory: NodeFactoryCallable, *args: typing.Any, **kwargs: typing.Any) -> rclpy.node.Node: + def load( + self, factory: NodeFactoryCallable, *args: typing.Any, **kwargs: typing.Any + ) -> rclpy.node.Node: """ Instantiates and loads a ROS 2 node. @@ -359,7 +375,9 @@ def load( namespace = namespace_with(self._namespace, namespace) else: namespace = self._namespace - node_or_graph = factory(*args, context=self._context, namespace=namespace, **kwargs) + node_or_graph = factory( + *args, context=self._context, namespace=namespace, **kwargs + ) if not isinstance(node_or_graph, rclpy.node.Node): graph = list(node_or_graph) for node in graph: @@ -409,7 +427,9 @@ def spin(self) -> None: """ @typing.overload - def spin(self, factory: NodeFactoryCallable, *args: typing.Any, **kwargs: typing.Any) -> None: + def spin( + self, factory: NodeFactoryCallable, *args: typing.Any, **kwargs: typing.Any + ) -> None: """ Spins scope executor (and all nodes in it). @@ -429,7 +449,9 @@ def spin(self, factory: NodeFactoryCallable, *args: typing.Any, **kwargs: typing """ @typing.overload - def spin(self, factory: GraphFactoryCallable, *args: typing.Any, **kwargs: typing.Any) -> None: + def spin( + self, factory: GraphFactoryCallable, *args: typing.Any, **kwargs: typing.Any + ) -> None: """ Spins scope executor (and all nodes in it). @@ -448,7 +470,10 @@ def spin(self, factory: GraphFactoryCallable, *args: typing.Any, **kwargs: typin """ def spin( - self, factory: typing.Optional[AnyEntityFactoryCallable] = None, *args: typing.Any, **kwargs: typing.Any + self, + factory: typing.Optional[AnyEntityFactoryCallable] = None, + *args: typing.Any, + **kwargs: typing.Any, ) -> None: if self._stack is None: raise RuntimeError("scope has not been entered (or already exited)") @@ -456,9 +481,15 @@ def spin( raise RuntimeError("scope executor already spinning") with self._lock: if self._executor is None: - logger = rclpy.logging.get_logger(self._namespace or fqn(self.__class__)) + logger = rclpy.logging.get_logger( + self._namespace or fqn(self.__class__) + ) self._executor = self._stack.enter_context( - foreground(AutoScalingMultiThreadedExecutor(logger=logger, context=self._context)) + foreground( + AutoScalingMultiThreadedExecutor( + logger=logger, context=self._context + ) + ) ) with contextlib.ExitStack() as stack: if factory is not None: @@ -529,7 +560,9 @@ def executor() -> typing.Optional[rclpy.executors.Executor]: return scope.executor -def load(factory: AnyEntityFactoryCallable, *args: typing.Any, **kwargs: typing.Any) -> AnyEntity: +def load( + factory: AnyEntityFactoryCallable, *args: typing.Any, **kwargs: typing.Any +) -> AnyEntity: """ Loads a ROS 2 node (or a collection thereof) within the current ROS 2 aware scope. @@ -579,7 +612,11 @@ def managed( return scope.managed(factory, *args, **kwargs) -def spin(factory: typing.Optional[AnyEntityFactoryCallable] = None, *args: typing.Any, **kwargs: typing.Any) -> None: +def spin( + factory: typing.Optional[AnyEntityFactoryCallable] = None, + *args: typing.Any, + **kwargs: typing.Any, +) -> None: """ Spins current ROS 2 aware scope executor (and all the ROS 2 nodes in it). diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/service_handle.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/service_handle.py index d6b0f50..ccaf3f9 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/service_handle.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/service_handle.py @@ -15,7 +15,12 @@ class ServiceHandle(object): as holding the various callbacks for sending an ServiceRequest (result, failure) """ - def __init__(self, service_name: str, logger: Optional[RcutilsLogger] = None, context: Optional[Context] = None): + def __init__( + self, + service_name: str, + logger: Optional[RcutilsLogger] = None, + context: Optional[Context] = None, + ): if context is None: context = get_default_context() self._service_name = service_name @@ -73,7 +78,9 @@ def _service_result_callback(self, future: Future) -> None: service_has_success = hasattr(result, "success") if self._on_failure_callback is not None and not service_has_success: - self._logger.warning("Failure callback is set, but result has no success attribute") + self._logger.warning( + "Failure callback is set, but result has no success attribute" + ) self._future_ready_event.set() return @@ -84,7 +91,9 @@ def _service_result_callback(self, future: Future) -> None: self._future_ready_event.set() return else: - self._logger.warning(f"Service {self._service_name} has no success or failure flag") + self._logger.warning( + f"Service {self._service_name} has no success or failure flag" + ) self._logger.info("Service completed") self._future_ready_event.set() diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/single_goal_action_server.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/single_goal_action_server.py index de8b327..ef6bbfd 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/single_goal_action_server.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/single_goal_action_server.py @@ -5,7 +5,9 @@ from rclpy.callback_groups import CallbackGroup from rclpy.node import Node -from bdai_ros2_wrappers.single_goal_multiple_action_servers import SingleGoalMultipleActionServers +from bdai_ros2_wrappers.single_goal_multiple_action_servers import ( + SingleGoalMultipleActionServers, +) from bdai_ros2_wrappers.type_hints import ActionType # Note: for this to work correctly you must use a multi-threaded executor when spinning the node! E.g.: @@ -16,7 +18,8 @@ class SingleGoalActionServer(SingleGoalMultipleActionServers): """Wrapper around a single action server that only allows a single Action to be executing at one time. If a new - Action.Goal is received, the existing Action (if there is one) is preemptively canceled""" + Action.Goal is received, the existing Action (if there is one) is preemptively canceled + """ def __init__( self, @@ -27,5 +30,8 @@ def __init__( callback_group: Optional[CallbackGroup] = None, ) -> None: super().__init__( - node=node, action_server_parameters=[(action_type, action_topic, execute_callback, callback_group)] + node=node, + action_server_parameters=[ + (action_type, action_topic, execute_callback, callback_group) + ], ) diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/single_goal_multiple_action_servers.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/single_goal_multiple_action_servers.py index b8c458f..75e7238 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/single_goal_multiple_action_servers.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/single_goal_multiple_action_servers.py @@ -17,13 +17,22 @@ class SingleGoalMultipleActionServers(object): canceled""" def __init__( - self, node: Node, action_server_parameters: List[Tuple[ActionType, str, Callable, Optional[CallbackGroup]]] + self, + node: Node, + action_server_parameters: List[ + Tuple[ActionType, str, Callable, Optional[CallbackGroup]] + ], ) -> None: self._node = node self._goal_handle: Optional[ServerGoalHandle] = None self._goal_lock = threading.Lock() self._action_servers = [] - for action_type, action_topic, execute_callback, callback_group in action_server_parameters: + for ( + action_type, + action_topic, + execute_callback, + callback_group, + ) in action_server_parameters: self._action_servers.append( ActionServer( node, diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/static_transform_broadcaster.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/static_transform_broadcaster.py index 392f56f..f5a3f47 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/static_transform_broadcaster.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/static_transform_broadcaster.py @@ -13,9 +13,15 @@ class StaticTransformBroadcaster: sent through it, matching ``rclcpp::StaticTransformBroadcaster`` behavior. """ - DEFAULT_QOS = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST) + DEFAULT_QOS = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ) - def __init__(self, node: Node, qos: Optional[Union[QoSProfile, int]] = None) -> None: + def __init__( + self, node: Node, qos: Optional[Union[QoSProfile, int]] = None + ) -> None: """ Constructor. @@ -27,7 +33,9 @@ def __init__(self, node: Node, qos: Optional[Union[QoSProfile, int]] = None) -> self._net_transforms: Dict[str, TransformStamped] = {} self._pub = node.create_publisher(TFMessage, "/tf_static", qos) - def sendTransform(self, transform: Union[TransformStamped, Iterable[TransformStamped]]) -> None: + def sendTransform( + self, transform: Union[TransformStamped, Iterable[TransformStamped]] + ) -> None: try: transforms = list(transform) except TypeError: # in which case, transform is not iterable diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/subscription.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/subscription.py index e1d00fb..5db84f8 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/subscription.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/subscription.py @@ -36,7 +36,9 @@ def wait_for_message_async( """ node = node or scope.node() if node is None: - raise ValueError("no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)") + raise ValueError( + "no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)" + ) future = rclpy.task.Future() def callback(msg: MessageT) -> None: @@ -72,7 +74,9 @@ def wait_for_message( """ node = node or scope.node() if node is None: - raise ValueError("no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)") + raise ValueError( + "no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)" + ) future = wait_for_message_async(msg_type, topic_name, node=node, **kwargs) if not wait_for_future(future, timeout_sec, context=node.context): future.cancel() diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/tf_listener_wrapper.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/tf_listener_wrapper.py index 9f03b7a..690119a 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/tf_listener_wrapper.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/tf_listener_wrapper.py @@ -62,7 +62,9 @@ def callback(self): # or you can lookup and handle exceptions yourself """ - def __init__(self, node: Optional[Node] = None, cache_time_s: Optional[float] = None) -> None: + def __init__( + self, node: Optional[Node] = None, cache_time_s: Optional[float] = None + ) -> None: """ Initializes the wrapper. @@ -72,7 +74,9 @@ def __init__(self, node: Optional[Node] = None, cache_time_s: Optional[float] = """ node = node or scope.node() if node is None: - raise ValueError("no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)") + raise ValueError( + "no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)" + ) self._node = node cache_time_py = None if cache_time_s is not None: @@ -88,7 +92,10 @@ def shutdown(self) -> None: self._tf_listener.unregister() def wait_for_a_tform_b_async( - self, frame_a: str, frame_b: str, transform_time: Optional[Union[StampLike, TimeLike]] = None + self, + frame_a: str, + frame_b: str, + transform_time: Optional[Union[StampLike, TimeLike]] = None, ) -> Future: """ Wait asynchronously for the transform from from_frame to to_frame to become available. @@ -103,7 +110,9 @@ def wait_for_a_tform_b_async( """ if transform_time is None: transform_time = Time() - return self._tf_buffer.wait_for_transform_async(frame_a, frame_b, transform_time) + return self._tf_buffer.wait_for_transform_async( + frame_a, frame_b, transform_time + ) def wait_for_a_tform_b( self, @@ -189,7 +198,11 @@ def lookup_a_tform_b( return self._tf_buffer.lookup_transform(frame_a, frame_b, transform_time) def lookup_latest_timestamp( - self, frame_a: str, frame_b: str, timeout_sec: Optional[float] = None, wait_for_frames: bool = False + self, + frame_a: str, + frame_b: str, + timeout_sec: Optional[float] = None, + wait_for_frames: bool = False, ) -> Time: """ Looks up the latest time at which a transform from frame_a to frame_b is available. @@ -209,5 +222,7 @@ def lookup_latest_timestamp( Raises: All the possible TransformExceptions. """ - transform = self.lookup_a_tform_b(frame_a, frame_b, Time(), timeout_sec, wait_for_frames) + transform = self.lookup_a_tform_b( + frame_a, frame_b, Time(), timeout_sec, wait_for_frames + ) return Time.from_msg(transform.header.stamp) diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/utilities.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/utilities.py index bb5b9c1..9778f2e 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/utilities.py +++ b/bdai_ros2_wrappers/bdai_ros2_wrappers/utilities.py @@ -45,7 +45,9 @@ def fqn(obj: typing.Any) -> typing.Optional[str]: return f"{module}.{name}" -def bind_to_thread(callable_: typing.Callable, thread: threading.Thread) -> typing.Callable: +def bind_to_thread( + callable_: typing.Callable, thread: threading.Thread +) -> typing.Callable: """Binds a callable to a thread, so it can only be invoked from that thread.""" @functools.wraps(callable_) diff --git a/bdai_ros2_wrappers/examples/background_action_example.py b/bdai_ros2_wrappers/examples/background_action_example.py index 2dd3798..7730f97 100644 --- a/bdai_ros2_wrappers/examples/background_action_example.py +++ b/bdai_ros2_wrappers/examples/background_action_example.py @@ -28,7 +28,9 @@ class MinimalActionServer(Node): def __init__(self, node_name: str = "minimal_action_server", **kwargs: Any) -> None: super().__init__(node_name, **kwargs) - self._action_server = ActionServer(self, Fibonacci, "compute_fibonacci_sequence", self.execute_callback) + self._action_server = ActionServer( + self, Fibonacci, "compute_fibonacci_sequence", self.execute_callback + ) def execute_callback(self, goal_handle: ServerGoalHandle) -> Fibonacci.Result: sequence = [0, 1] @@ -67,7 +69,9 @@ def main() -> None: assert action_client.wait_for_server(timeout_sec=5) while main.context.ok(): - line = input("Please provide a Fibonacci sequence order (or press Enter to exit): ") + line = input( + "Please provide a Fibonacci sequence order (or press Enter to exit): " + ) if not line: break try: diff --git a/bdai_ros2_wrappers/examples/talker_listener_example.py b/bdai_ros2_wrappers/examples/talker_listener_example.py index 5834a14..dd398ce 100644 --- a/bdai_ros2_wrappers/examples/talker_listener_example.py +++ b/bdai_ros2_wrappers/examples/talker_listener_example.py @@ -30,7 +30,9 @@ def __init__(self, **kwargs: typing.Any) -> None: self.timer = self.create_timer(1 / rate, self.callback) def callback(self) -> None: - message = std_msgs.msg.String(data=f"Hi there, from {self.get_name()} (#{next(self.counter)})") + message = std_msgs.msg.String( + data=f"Hi there, from {self.get_name()} (#{next(self.counter)})" + ) self.pub.publish(message) @@ -39,7 +41,9 @@ class ListenerNode(Node): def __init__(self, **kwargs: typing.Any) -> None: super().__init__("listener", **kwargs) - self.sub = self.create_subscription(std_msgs.msg.String, "chat", self.callback, 1) + self.sub = self.create_subscription( + std_msgs.msg.String, "chat", self.callback, 1 + ) def callback(self, message: std_msgs.msg.String) -> None: self.get_logger().info(message.data) diff --git a/bdai_ros2_wrappers/examples/tf_cli_example.py b/bdai_ros2_wrappers/examples/tf_cli_example.py index fe04470..cde27b9 100644 --- a/bdai_ros2_wrappers/examples/tf_cli_example.py +++ b/bdai_ros2_wrappers/examples/tf_cli_example.py @@ -33,7 +33,10 @@ def __init__(self, tf_prefix: Optional[str] = None, **kwargs: Any) -> None: super().__init__("tf_broadcaster", **kwargs) self.tf_broadcaster = TransformBroadcaster(self) self.transforms: List[TransformStamped] = [] - tree = [("world", "odom", 0.0, 1.0, math.degrees(90.0)), ("odom", "robot", 3.0, -0.5, math.degrees(-30.0))] + tree = [ + ("world", "odom", 0.0, 1.0, math.degrees(90.0)), + ("odom", "robot", 3.0, -0.5, math.degrees(-30.0)), + ] for parent_frame, child_frame, x, y, theta in tree: transform = TransformStamped() transform.header.frame_id = namespace_with(tf_prefix, parent_frame) @@ -83,7 +86,9 @@ def run(args: argparse.Namespace) -> None: source_frame = target_frame target_frame = namespace_with(args.tf_prefix, target_frame) source_frame = namespace_with(args.tf_prefix, source_frame) - transform = tf_listener.lookup_a_tform_b(target_frame, source_frame, wait_for_frames=True) + transform = tf_listener.lookup_a_tform_b( + target_frame, source_frame, wait_for_frames=True + ) t = transform.transform.translation q = transform.transform.rotation print(f"Transform {target_frame} -> {source_frame}") @@ -111,7 +116,9 @@ def main(args: argparse.Namespace) -> None: """ with background(SingleThreadedExecutor()) as main.executor: with ros_process.managed(graph, args) as (_, main.node): - main.tf_listener = TFListenerWrapper(main.node, cache_time_s=args.cache_time) + main.tf_listener = TFListenerWrapper( + main.node, cache_time_s=args.cache_time + ) run(args) diff --git a/bdai_ros2_wrappers/package.xml b/bdai_ros2_wrappers/package.xml index 9a34703..127b706 100644 --- a/bdai_ros2_wrappers/package.xml +++ b/bdai_ros2_wrappers/package.xml @@ -2,7 +2,7 @@ bdai_ros2_wrappers - 0.0.0 + 0.0.1 BDAII wrappers for ROS2 BD AI Institute MIT diff --git a/bdai_ros2_wrappers/setup.py b/bdai_ros2_wrappers/setup.py index 2985a12..0162661 100644 --- a/bdai_ros2_wrappers/setup.py +++ b/bdai_ros2_wrappers/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="0.0.0", + version="0.0.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -14,9 +14,9 @@ ], install_requires=["setuptools"], zip_safe=True, - maintainer="BD AI Institute", + maintainer="The AI Institute", maintainer_email="engineering@theaiinstitute.com", - description="BDAII wrappers for ROS2", + description="The AI Institute's wrappers for ROS2", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/bdai_ros2_wrappers/test/test_action_client.py b/bdai_ros2_wrappers/test/test_action_client.py index 54897fe..66220e4 100644 --- a/bdai_ros2_wrappers/test/test_action_client.py +++ b/bdai_ros2_wrappers/test/test_action_client.py @@ -49,7 +49,9 @@ def test_send_goal_and_wait(ros: ROSAwareScope) -> None: goal = Fibonacci.Goal() goal.order = 5 - result = action_client.send_goal_and_wait("test_send_goal_and_wait", goal=goal, timeout_sec=5) + result = action_client.send_goal_and_wait( + "test_send_goal_and_wait", goal=goal, timeout_sec=5 + ) assert result is not None expected_result = array.array("i", [0, 1, 1, 2, 3, 5]) assert result.sequence == expected_result @@ -65,7 +67,9 @@ def test_timeout_send_goal_wait(ros: ROSAwareScope) -> None: goal = Fibonacci.Goal() goal.order = 5 - result = action_client.send_goal_and_wait("test_timeout_send_goal_wait", goal=goal, timeout_sec=0.5) + result = action_client.send_goal_and_wait( + "test_timeout_send_goal_wait", goal=goal, timeout_sec=0.5 + ) assert result is None @@ -86,5 +90,7 @@ def do_not_accept_goal(goal_request: Fibonacci.Goal) -> GoalResponse: goal = Fibonacci.Goal() goal.order = 5 - result = action_client.send_goal_and_wait("test_goal_not_accepted", goal=goal, timeout_sec=5) + result = action_client.send_goal_and_wait( + "test_goal_not_accepted", goal=goal, timeout_sec=5 + ) assert result is None diff --git a/bdai_ros2_wrappers/test/test_action_handle.py b/bdai_ros2_wrappers/test/test_action_handle.py index eb815a3..35c393e 100644 --- a/bdai_ros2_wrappers/test/test_action_handle.py +++ b/bdai_ros2_wrappers/test/test_action_handle.py @@ -40,7 +40,9 @@ def _execute_callback_feedback(goal_handle: ServerGoalHandle) -> Fibonacci.Resul time.sleep(0.001) for i in range(1, goal_handle.request.order): - feedback.partial_sequence.append(feedback.partial_sequence[i] + feedback.partial_sequence[i - 1]) + feedback.partial_sequence.append( + feedback.partial_sequence[i] + feedback.partial_sequence[i - 1] + ) goal_handle.publish_feedback(feedback) time.sleep(0.01) @@ -80,7 +82,11 @@ class FibonacciActionServer(ActionServer): """ def __init__( - self, node: Node, name: str, execute_callback: Callable = _default_execute_callback, **kwargs: Any + self, + node: Node, + name: str, + execute_callback: Callable = _default_execute_callback, + **kwargs: Any ) -> None: super().__init__(node, Fibonacci, name, execute_callback, **kwargs) @@ -122,7 +128,9 @@ def do_send_goal( handle.set_on_cancel_success_callback(mocks.cancel_success_callback) handle.set_on_cancel_failure_callback(mocks.cancel_failure_callback) - send_goal_future = action_client.send_goal_async(goal, feedback_callback=handle.get_feedback_callback) + send_goal_future = action_client.send_goal_async( + goal, feedback_callback=handle.get_feedback_callback + ) handle.set_send_goal_future(send_goal_future) return handle, mocks @@ -160,7 +168,9 @@ def test_reject(ros: ROSAwareScope) -> None: def test_abort(ros: ROSAwareScope) -> None: """Tests the case where the action server aborts the goal""" assert ros.node is not None - FibonacciActionServer(ros.node, "fibonacci_ah", execute_callback=_execute_callback_abort) + FibonacciActionServer( + ros.node, "fibonacci_ah", execute_callback=_execute_callback_abort + ) action_client = ActionClient(ros.node, Fibonacci, "fibonacci_ah") handle, mocks = do_send_goal(action_client, Fibonacci.Goal(order=5)) assert not handle.wait_for_result(timeout_sec=1.0) @@ -174,7 +184,9 @@ def test_abort(ros: ROSAwareScope) -> None: def test_feedback(ros: ROSAwareScope) -> None: assert ros.node is not None - FibonacciActionServer(ros.node, "fibonacci_ah", execute_callback=_execute_callback_feedback) + FibonacciActionServer( + ros.node, "fibonacci_ah", execute_callback=_execute_callback_feedback + ) action_client = ActionClient(ros.node, Fibonacci, "fibonacci_ah") handle, mocks = do_send_goal(action_client, Fibonacci.Goal(order=5)) assert handle.wait_for_result(timeout_sec=1.0) @@ -209,7 +221,9 @@ def test_cancel_success(ros: ROSAwareScope) -> None: def test_cancel_failure(ros: ROSAwareScope) -> None: assert ros.node is not None - FibonacciActionServer(ros.node, "fibonacci_ah", execute_callback=_execute_callback_slowly) + FibonacciActionServer( + ros.node, "fibonacci_ah", execute_callback=_execute_callback_slowly + ) action_client = ActionClient(ros.node, Fibonacci, "fibonacci_ah") handle, mocks = do_send_goal(action_client, Fibonacci.Goal(order=5)) assert handle.wait_for_acceptance(timeout_sec=1.0) @@ -226,7 +240,9 @@ def test_cancel_failure(ros: ROSAwareScope) -> None: def test_wait_for_result_timeout(ros: ROSAwareScope) -> None: """Tests the wait_for_result function for when it times out""" assert ros.node is not None - FibonacciActionServer(ros.node, "fibonacci_ah", execute_callback=_execute_callback_slowly) + FibonacciActionServer( + ros.node, "fibonacci_ah", execute_callback=_execute_callback_slowly + ) action_client = ActionClient(ros.node, Fibonacci, "fibonacci_ah") handle, mocks = do_send_goal(action_client, Fibonacci.Goal(order=5)) assert not handle.wait_for_result(timeout_sec=0.01) @@ -259,7 +275,9 @@ def test_wait_for_result_cancelled(ros: ROSAwareScope) -> None: def test_wait_for_result_aborted(ros: ROSAwareScope) -> None: """Test if the ActionServer cancels the request""" assert ros.node is not None - FibonacciActionServer(ros.node, "fibonacci_ah", execute_callback=_execute_callback_abort) + FibonacciActionServer( + ros.node, "fibonacci_ah", execute_callback=_execute_callback_abort + ) action_client = ActionClient(ros.node, Fibonacci, "fibonacci_ah") handle, mocks = do_send_goal(action_client, Fibonacci.Goal(order=5)) # Check the return value and make sure the result is both executed to completion and returns negatively diff --git a/bdai_ros2_wrappers/test/test_executors.py b/bdai_ros2_wrappers/test/test_executors.py index 2033447..58b2b16 100644 --- a/bdai_ros2_wrappers/test/test_executors.py +++ b/bdai_ros2_wrappers/test/test_executors.py @@ -11,7 +11,11 @@ from rclpy.node import Node from std_srvs.srv import Trigger -from bdai_ros2_wrappers.executors import AutoScalingMultiThreadedExecutor, AutoScalingThreadPool, background +from bdai_ros2_wrappers.executors import ( + AutoScalingMultiThreadedExecutor, + AutoScalingThreadPool, + background, +) from bdai_ros2_wrappers.futures import wait_for_future @@ -126,7 +130,9 @@ def test_autoscaling_thread_pool_with_quota() -> None: Asserts that the autoscaling thread pool respects submission quotas. """ - with AutoScalingThreadPool(submission_quota=5, submission_patience=1.0, max_idle_time=2.0) as pool: + with AutoScalingThreadPool( + submission_quota=5, submission_patience=1.0, max_idle_time=2.0 + ) as pool: assert len(pool.workers) == 0 assert not pool.working @@ -162,15 +168,22 @@ def test_autoscaling_executor(ros_context: Context, ros_node: Node) -> None: the same executor. """ - def dummy_server_callback(_: Trigger.Request, response: Trigger.Response) -> Trigger.Response: + def dummy_server_callback( + _: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: response.success = True return response ros_node.create_service( - Trigger, "/dummy/trigger", dummy_server_callback, callback_group=MutuallyExclusiveCallbackGroup() + Trigger, + "/dummy/trigger", + dummy_server_callback, + callback_group=MutuallyExclusiveCallbackGroup(), ) - client = ros_node.create_client(Trigger, "/dummy/trigger", callback_group=MutuallyExclusiveCallbackGroup()) + client = ros_node.create_client( + Trigger, "/dummy/trigger", callback_group=MutuallyExclusiveCallbackGroup() + ) executor = AutoScalingMultiThreadedExecutor(context=ros_context) assert len(executor.thread_pool.workers) == 0 diff --git a/bdai_ros2_wrappers/test/test_integration.py b/bdai_ros2_wrappers/test/test_integration.py index 4101466..6580ad7 100644 --- a/bdai_ros2_wrappers/test/test_integration.py +++ b/bdai_ros2_wrappers/test/test_integration.py @@ -24,7 +24,9 @@ class MinimalTransformPublisher(Node): frame_id = "world" child_frame_id = "robot" - def __init__(self, node_name: str = "minimal_transform_publisher", **kwargs: Any) -> None: + def __init__( + self, node_name: str = "minimal_transform_publisher", **kwargs: Any + ) -> None: super().__init__(node_name, **kwargs) self._broadcaster = tf2_ros.TransformBroadcaster(self) self._transform = TransformStamped() @@ -49,9 +51,13 @@ class MinimalServer(Node): def __init__(self, node_name: str = "minimal_server", **kwargs: Any) -> None: super().__init__(node_name, **kwargs) - self._service_server = self.create_service(AddTwoInts, "add_two_ints", self.service_callback) + self._service_server = self.create_service( + AddTwoInts, "add_two_ints", self.service_callback + ) - def service_callback(self, request: AddTwoInts.Request, response: AddTwoInts.Response) -> AddTwoInts.Response: + def service_callback( + self, request: AddTwoInts.Request, response: AddTwoInts.Response + ) -> AddTwoInts.Response: response.sum = request.a + request.b return response @@ -61,7 +67,9 @@ class MinimalActionServer(Node): def __init__(self, node_name: str = "minimal_action_server", **kwargs: Any) -> None: super().__init__(node_name, **kwargs) - self._action_server = ActionServer(self, Fibonacci, "compute_fibonacci_sequence", self.execute_callback) + self._action_server = ActionServer( + self, Fibonacci, "compute_fibonacci_sequence", self.execute_callback + ) def execute_callback(self, goal_handle: ServerGoalHandle) -> Fibonacci.Result: sequence = [0, 1] @@ -109,7 +117,8 @@ def blocking_sequence() -> TransformStamped: assert action_client.wait_for_server(timeout_sec=5) feedback = [] result = action_client.send_goal( - Fibonacci.Goal(order=response.sum), feedback_callback=lambda f: feedback.append(f.feedback) + Fibonacci.Goal(order=response.sum), + feedback_callback=lambda f: feedback.append(f.feedback), ).result assert len(feedback) > 0 partial_sequence = feedback[-1].sequence @@ -117,14 +126,20 @@ def blocking_sequence() -> TransformStamped: timeout = Duration(seconds=5) assert tf_buffer.can_transform( - MinimalTransformPublisher.frame_id, MinimalTransformPublisher.child_frame_id, Time(), timeout + MinimalTransformPublisher.frame_id, + MinimalTransformPublisher.child_frame_id, + Time(), + timeout, ) assert ros.node is not None time = ros.node.get_clock().now() time += Duration(seconds=result.sequence[-1] * 10e-3) return tf_buffer.lookup_transform( - MinimalTransformPublisher.frame_id, MinimalTransformPublisher.child_frame_id, time, timeout + MinimalTransformPublisher.frame_id, + MinimalTransformPublisher.child_frame_id, + time, + timeout, ) assert ros.executor is not None @@ -154,7 +169,11 @@ def add_fibonacci_sequences_server_callback( response.sum = sum(result_a.sequence) + sum(result_b.sequence) return response - ros.node.create_service(AddTwoInts, "add_two_fibonacci_sequences", add_fibonacci_sequences_server_callback) + ros.node.create_service( + AddTwoInts, + "add_two_fibonacci_sequences", + add_fibonacci_sequences_server_callback, + ) client = ros.node.create_client(AddTwoInts, "add_two_fibonacci_sequences") diff --git a/bdai_ros2_wrappers/test/test_process.py b/bdai_ros2_wrappers/test/test_process.py index 3e367e3..fcd695c 100644 --- a/bdai_ros2_wrappers/test/test_process.py +++ b/bdai_ros2_wrappers/test/test_process.py @@ -14,7 +14,9 @@ def test_process_wrapping() -> None: @process.main() def main() -> int: - def dummy_server_callback(_: Trigger.Request, response: Trigger.Response) -> Trigger.Response: + def dummy_server_callback( + _: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: response.success = True return response @@ -48,7 +50,9 @@ def main() -> int: tf_broadcaster.sendTransform(expected_transform) assert main.tf_listener is not None - transform = main.tf_listener.lookup_a_tform_b("world", "robot", stamp, timeout_sec=5.0, wait_for_frames=True) + transform = main.tf_listener.lookup_a_tform_b( + "world", "robot", stamp, timeout_sec=5.0, wait_for_frames=True + ) assert transform.header.stamp == expected_transform.header.stamp assert transform.transform.rotation.w == expected_transform.transform.rotation.w return 0 @@ -81,7 +85,9 @@ def cli() -> argparse.ArgumentParser: parser = argparse.ArgumentParser("test_command") parser.add_argument("robot") parser.add_argument("-q", "--quiet", action="store_true") - parser.set_defaults(process_args=lambda args: dict(forward_logging=not args.quiet)) + parser.set_defaults( + process_args=lambda args: dict(forward_logging=not args.quiet) + ) return parser @process.main(cli(), namespace="{robot}") diff --git a/bdai_ros2_wrappers/test/test_service_handle.py b/bdai_ros2_wrappers/test/test_service_handle.py index e5cd1ea..763d894 100644 --- a/bdai_ros2_wrappers/test/test_service_handle.py +++ b/bdai_ros2_wrappers/test/test_service_handle.py @@ -74,7 +74,9 @@ def callback(req: SrvTypeRequest, resp: SrvTypeResponse) -> SrvTypeResponse: assert result.message == "bar" -def setbool_service_callback(req: SrvTypeRequest, resp: SrvTypeResponse) -> SrvTypeResponse: +def setbool_service_callback( + req: SrvTypeRequest, resp: SrvTypeResponse +) -> SrvTypeResponse: result = SetBool.Response() if req.data: result.success = True diff --git a/bdai_ros2_wrappers/test/test_single_goal_action_server.py b/bdai_ros2_wrappers/test/test_single_goal_action_server.py index 0c075b6..141eb55 100644 --- a/bdai_ros2_wrappers/test/test_single_goal_action_server.py +++ b/bdai_ros2_wrappers/test/test_single_goal_action_server.py @@ -34,7 +34,9 @@ def execute_callback(goal_handle: ServerGoalHandle) -> Fibonacci.Result: goal = Fibonacci.Goal() goal.order = 5 - result = action_client.send_goal_and_wait("test_single_goal_action_server", goal=goal, timeout_sec=2) + result = action_client.send_goal_and_wait( + "test_single_goal_action_server", goal=goal, timeout_sec=2 + ) assert result is not None expected_result = array.array("i", [0, 1, 1, 2, 3, 5]) diff --git a/bdai_ros2_wrappers/test/test_single_goal_multiple_action_servers.py b/bdai_ros2_wrappers/test/test_single_goal_multiple_action_servers.py index aae3abf..b0ab2bd 100644 --- a/bdai_ros2_wrappers/test/test_single_goal_multiple_action_servers.py +++ b/bdai_ros2_wrappers/test/test_single_goal_multiple_action_servers.py @@ -9,7 +9,9 @@ from bdai_ros2_wrappers.action_client import ActionClientWrapper from bdai_ros2_wrappers.scope import ROSAwareScope -from bdai_ros2_wrappers.single_goal_multiple_action_servers import SingleGoalMultipleActionServers +from bdai_ros2_wrappers.single_goal_multiple_action_servers import ( + SingleGoalMultipleActionServers, +) def execute_callback(goal_handle: ServerGoalHandle) -> Fibonacci.Result: @@ -65,7 +67,9 @@ def action_triplet( def test_actions_in_sequence( - action_triplet: Tuple[SingleGoalMultipleActionServers, ActionClientWrapper, ActionClientWrapper] + action_triplet: Tuple[ + SingleGoalMultipleActionServers, ActionClientWrapper, ActionClientWrapper + ] ) -> None: """ Tests out normal operation with multiple action servers and clients @@ -74,19 +78,26 @@ def test_actions_in_sequence( goal = Fibonacci.Goal() goal.order = 5 # use first client - result = action_client_a.send_goal_and_wait("action_request_a", goal=goal, timeout_sec=5) + result = action_client_a.send_goal_and_wait( + "action_request_a", goal=goal, timeout_sec=5 + ) assert result is not None expected_result = array.array("i", [0, 1, 1, 2, 3, 5]) assert result.sequence == expected_result # use second client - result = action_client_b.send_goal_and_wait("action_request_b", goal=goal, timeout_sec=5) + result = action_client_b.send_goal_and_wait( + "action_request_b", goal=goal, timeout_sec=5 + ) assert result is not None expected_result = array.array("i", [0, 1, 0, 0, 0, 0]) assert result.sequence == expected_result def test_action_interruption( - ros: ROSAwareScope, action_triplet: Tuple[SingleGoalMultipleActionServers, ActionClientWrapper, ActionClientWrapper] + ros: ROSAwareScope, + action_triplet: Tuple[ + SingleGoalMultipleActionServers, ActionClientWrapper, ActionClientWrapper + ], ) -> None: """ This test should start a delayed request from another client @@ -102,7 +113,9 @@ def deferred_request() -> None: time.sleep(0.3) goal = Fibonacci.Goal() goal.order = 5 - action_client_a.send_goal_and_wait("deferred_action_request", goal=goal, timeout_sec=2) + action_client_a.send_goal_and_wait( + "deferred_action_request", goal=goal, timeout_sec=2 + ) assert ros.executor is not None ros.executor.create_task(deferred_request) @@ -110,5 +123,7 @@ def deferred_request() -> None: # immediately start the request for other goal goal = Fibonacci.Goal() goal.order = 5 - result = action_client_b.send_goal_and_wait("action_request", goal=goal, timeout_sec=5) + result = action_client_b.send_goal_and_wait( + "action_request", goal=goal, timeout_sec=5 + ) assert result is None diff --git a/bdai_ros2_wrappers/test/test_static_transform_broadcaster.py b/bdai_ros2_wrappers/test/test_static_transform_broadcaster.py index 744e3c6..740fd56 100644 --- a/bdai_ros2_wrappers/test/test_static_transform_broadcaster.py +++ b/bdai_ros2_wrappers/test/test_static_transform_broadcaster.py @@ -47,10 +47,16 @@ def test_static_tf_burst(ros: ROSAwareScope) -> None: footprint_to_body_transform.header.frame_id = "footprint" footprint_to_body_transform.child_frame_id = "body" footprint_to_body_transform.transform.rotation.w = 1.0 - tf_broadcaster.sendTransform([world_to_fiducial_a_transform, footprint_to_body_transform]) + tf_broadcaster.sendTransform( + [world_to_fiducial_a_transform, footprint_to_body_transform] + ) tf_listener = TFListenerWrapper(ros.node) - transform = tf_listener.lookup_a_tform_b("footprint", "camera", timeout_sec=2.0, wait_for_frames=True) + transform = tf_listener.lookup_a_tform_b( + "footprint", "camera", timeout_sec=2.0, wait_for_frames=True + ) assert transform.transform.rotation.w == 1.0 - transform = tf_listener.lookup_a_tform_b("world", "fiducial_a", timeout_sec=2.0, wait_for_frames=True) + transform = tf_listener.lookup_a_tform_b( + "world", "fiducial_a", timeout_sec=2.0, wait_for_frames=True + ) assert transform.transform.translation.x == 1.0 diff --git a/bdai_ros2_wrappers/test/test_tf_listener_wrapper.py b/bdai_ros2_wrappers/test/test_tf_listener_wrapper.py index 3a6e6a9..0c7a2eb 100644 --- a/bdai_ros2_wrappers/test/test_tf_listener_wrapper.py +++ b/bdai_ros2_wrappers/test/test_tf_listener_wrapper.py @@ -52,14 +52,18 @@ def publish_transform(self, trans: Transform, timestamp: Optional[Time]) -> None @pytest.fixture -def tf_pair(ros: ROSAwareScope) -> Iterable[Tuple[MockTfPublisherNode, TFListenerWrapper]]: +def tf_pair( + ros: ROSAwareScope, +) -> Iterable[Tuple[MockTfPublisherNode, TFListenerWrapper]]: with ros.managed(MockTfPublisherNode, FRAME_ID, CHILD_FRAME_ID) as tf_publisher: assert ros.node is not None tf_listener = TFListenerWrapper(ros.node) yield tf_publisher, tf_listener -def test_non_existant_transform(ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper]) -> None: +def test_non_existant_transform( + ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper] +) -> None: tf_publisher, tf_listener = tf_pair assert ros.node is not None timestamp = ros.node.get_clock().now() @@ -75,15 +79,22 @@ def test_non_existant_transform_timeout( timestamp = ros.node.get_clock().now() start = time.time() with pytest.raises(LookupException): - tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=20.0) + tf_listener.lookup_a_tform_b( + FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=20.0 + ) assert time.time() - start < 10.0 -def test_existing_transform(ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper]) -> None: +def test_existing_transform( + ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper] +) -> None: tf_publisher, tf_listener = tf_pair assert ros.node is not None timestamp = ros.node.get_clock().now() - trans = Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)) + trans = Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0), + ) tf_publisher.publish_transform(trans, timestamp) time.sleep(0.2) t = tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp) @@ -96,12 +107,17 @@ def test_future_transform_extrapolation_exception( tf_publisher, tf_listener = tf_pair assert ros.node is not None timestamp = ros.node.get_clock().now() - trans = Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)) + trans = Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0), + ) tf_publisher.publish_transform(trans, timestamp) time.sleep(0.2) timestamp = ros.node.get_clock().now() with pytest.raises(ExtrapolationException): - tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=0.0) + tf_listener.lookup_a_tform_b( + FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=0.0 + ) def test_future_transform_insufficient_wait( @@ -110,7 +126,10 @@ def test_future_transform_insufficient_wait( tf_publisher, tf_listener = tf_pair assert ros.node is not None timestamp = ros.node.get_clock().now() - trans = Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)) + trans = Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0), + ) tf_publisher.publish_transform(trans, timestamp) delay = 2 @@ -127,14 +146,21 @@ def delayed_publish() -> None: time.sleep(0.2) timestamp = ros.node.get_clock().now() + Duration(seconds=delay) with pytest.raises(ExtrapolationException): - tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=0.5) + tf_listener.lookup_a_tform_b( + FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=0.5 + ) -def test_future_transform_wait(ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper]) -> None: +def test_future_transform_wait( + ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper] +) -> None: tf_publisher, tf_listener = tf_pair assert ros.node is not None timestamp = ros.node.get_clock().now() - trans = Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0)) + trans = Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0), + ) tf_publisher.publish_transform(trans, timestamp) delay = 1 @@ -148,11 +174,15 @@ def delayed_publish() -> None: ros.executor.create_task(delayed_publish) timestamp += Duration(seconds=delay) - t = tf_listener.lookup_a_tform_b(FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=2.0, wait_for_frames=True) + t = tf_listener.lookup_a_tform_b( + FRAME_ID, CHILD_FRAME_ID, timestamp, timeout_sec=2.0, wait_for_frames=True + ) assert equal_transform(t.transform, trans) -def test_future_timestamp(ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper]) -> None: +def test_future_timestamp( + ros: ROSAwareScope, tf_pair: Tuple[MockTfPublisherNode, TFListenerWrapper] +) -> None: tf_publisher, tf_listener = tf_pair timestamp = tf_publisher.get_clock().now() diff --git a/configs/black.toml b/configs/black.toml new file mode 100644 index 0000000..566d230 --- /dev/null +++ b/configs/black.toml @@ -0,0 +1,13 @@ +[black] +line-length = 120 +target-version = ['py310'] +include = '\.pyi?$' +# `extend-exclude` is not honored when `black` is passed a file path explicitly, +# as is typical when `black` is invoked via `pre-commit`. +force-exclude = ''' +/( + docker/ros/.* +)/ +''' + +preview = true diff --git a/configs/mypy.ini b/configs/mypy.ini new file mode 100644 index 0000000..403c404 --- /dev/null +++ b/configs/mypy.ini @@ -0,0 +1,15 @@ +[mypy] +mypy_path = $MYPY_CONFIG_FILE_DIR/.. +python_version = 3.8 +disallow_untyped_defs = True +ignore_missing_imports = True +explicit_package_bases = True +check_untyped_defs = True +strict_equality = True +warn_unreachable = True +warn_redundant_casts = True +no_implicit_optional = True +files = + bdai_ros2_wrappers/bdai_ros2_wrappers, + bdai_ros2_wrappers/test +exclude = "^(docker|.*external|.*thirdparty|.*install|.*build|.*_experimental|.*conversions.py)/" diff --git a/configs/ruff.toml b/configs/ruff.toml new file mode 100644 index 0000000..3ef8f55 --- /dev/null +++ b/configs/ruff.toml @@ -0,0 +1,49 @@ +# Enable pycodestyle (`E`), Pyflakes (`F`), and import sorting (`I`) +select = ["E", "F", "I"] +ignore = [] + +# Allow autofix for all enabled rules (when `--fix`) is provided. +fixable = ["A", "B", "C", "D", "E", "F", "G", "I", "N", "Q", "S", "T", "W", "ANN", "ARG", "BLE", "COM", "DJ", "DTZ", "EM", "ERA", "EXE", "FBT", "ICN", "INP", "ISC", "NPY", "PD", "PGH", "PIE", "PL", "PT", "PTH", "PYI", "RET", "RSE", "RUF", "SIM", "SLF", "TCH", "TID", "TRY", "UP", "YTT"] +unfixable = [] + +# Exclude a variety of commonly ignored directories. +exclude = [ + ".bzr", + ".direnv", + ".eggs", + ".git", + ".hg", + ".hg", + ".mypy_cache", + ".nox", + ".pants.d", + ".pytype", + ".ruff_cache", + ".svn", + ".tox", + ".venv", + "__pypackages__", + "_build", + "buck-out", + "build", + "dist", + "node_modules", + "venv", + "docker/ros", +] + +# Same as Black. +line-length = 120 + +# Allow unused variables when underscore-prefixed. +dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" + +# Assume Python 3.10. +target-version = "py310" + +[per-file-ignores] +"__init__.py" = ["F401"] + +[mccabe] +# Unlike Flake8, default to a complexity level of 10. +max-complexity = 10 diff --git a/pyproject.toml b/pyproject.toml deleted file mode 100644 index 066eeb5..0000000 --- a/pyproject.toml +++ /dev/null @@ -1,98 +0,0 @@ -[build-system] -requires = ["setuptools>=61.0"] -build-backend = "setuptools.build_meta" - -[project] -name = "bdai_ros2_wrappers" -version = "1.0" -description = "Shared collection of wrappers for ROS2" -authors = [ - {name = "BD AI Institute", email = "engineering@theaiinstitute.com"}, -] -readme = "README.md" -requires-python = ">=3.10" - -[project.urls] -"Homepage" = "theaiinstitute.com" -"GitHub" = "https://github.com/bdaiinstitute/bdai" - -[tool.setuptools.packages.find] -where = ["bdai_ros2_wrappers"] - -[tool.ruff] -# Enable pycodestyle (`E`), Pyflakes (`F`), and import sorting (`I`) -select = ["E", "F", "I"] -ignore = [] - -# Allow autofix for all enabled rules (when `--fix`) is provided. -fixable = ["A", "B", "C", "D", "E", "F", "G", "I", "N", "Q", "S", "T", "W", "ANN", "ARG", "BLE", "COM", "DJ", "DTZ", "EM", "ERA", "EXE", "FBT", "ICN", "INP", "ISC", "NPY", "PD", "PGH", "PIE", "PL", "PT", "PTH", "PYI", "RET", "RSE", "RUF", "SIM", "SLF", "TCH", "TID", "TRY", "UP", "YTT"] -unfixable = [] - -# Exclude a variety of commonly ignored directories. -exclude = [ - ".bzr", - ".direnv", - ".eggs", - ".git", - ".hg", - ".hg", - ".mypy_cache", - ".nox", - ".pants.d", - ".pytype", - ".ruff_cache", - ".svn", - ".tox", - ".venv", - "__pypackages__", - "_build", - "buck-out", - "build", - "dist", - "node_modules", - "venv", - "docker/ros", -] - -# Same as Black. -line-length = 120 - -# Allow unused variables when underscore-prefixed. -dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" - -# Assume Python 3.10. -target-version = "py310" - -[tool.ruff.per-file-ignores] -"__init__.py" = ["F401"] - -[tool.ruff.mccabe] -# Unlike Flake8, default to a complexity level of 10. -max-complexity = 10 - -[tool.black] -line-length = 120 -target-version = ['py310'] -include = '\.pyi?$' -# `extend-exclude` is not honored when `black` is passed a file path explicitly, -# as is typical when `black` is invoked via `pre-commit`. -force-exclude = ''' -/( - docker/ros/.* -)/ -''' - -preview = true - -[tool.mypy] -python_version = "3.8" -disallow_untyped_defs = true -ignore_missing_imports = true -explicit_package_bases = true -check_untyped_defs = true -strict_equality = true -warn_unreachable = true -warn_redundant_casts = true -no_implicit_optional = true -files = ['bdai_ros2_wrappers'] -exclude = '^(docker|.*external|.*thirdparty|.*install|.*build|.*_experimental|.*conversions.py)/'