pntos-cobra 2.1.0.post0.dev0__py3-none-any.whl

pntos/__init__.py

@@ -0,0 +1 @@
+__path__ = __import__('pkgutil').extend_path(__path__, __name__)

pntos/cobra/__init__.py

@@ -0,0 +1,84 @@
+import contextlib
+
+with contextlib.suppress(ImportError):
+    from .advanced_plugins.Aspn23RosTransportPlugin import (
+        Aspn23RosTransportPlugin as Aspn23RosTransportPlugin,
+    )
+from .advanced_plugins.buscat.BuscatControllerPlugin import (
+    BuscatControllerPlugin as BuscatControllerPlugin,
+)
+from .advanced_plugins.ui.CobraUiLogPlayerPlugin import (
+    CobraUiLogPlayerPlugin as CobraUiLogPlayerPlugin,
+)
+from .advanced_plugins.ui.ExperimentalCobraUiPlugin import (
+    ExperimentalCobraUiPlugin as ExperimentalCobraUiPlugin,
+)
+from .dummy_plugins.DummyControllerPlugin import (
+    DummyControllerPlugin as DummyControllerPlugin,
+)
+from .dummy_plugins.DummyOrchestrationPlugin import (
+    DummyOrchestrationPlugin as DummyOrchestrationPlugin,
+)
+from .dummy_plugins.DummyTransportPlugin import (
+    DummyTransportPlugin as DummyTransportPlugin,
+)
+from .standard_plugins.controller.StandardControllerPlugin import (
+    StandardControllerPlugin as StandardControllerPlugin,
+)
+from .standard_plugins.DiagnosticLogPlugin import (
+    DiagnosticLogPlugin as DiagnosticLogPlugin,
+)
+from .standard_plugins.EkfFusionStrategyPlugin import (
+    EkfFusionStrategyPlugin as EkfFusionStrategyPlugin,
+)
+from .standard_plugins.fusion.StandardFusionPlugin import (
+    StandardFusionPlugin as StandardFusionPlugin,
+)
+from .standard_plugins.LcmLogTransportPlugin import (
+    LcmLogTransportPlugin as LcmLogTransportPlugin,
+)
+from .standard_plugins.LcmTransportPlugin import (
+    LcmTransportPlugin as LcmTransportPlugin,
+)
+from .standard_plugins.ManualHeadingAlignInitializationPlugin import (
+    ManualHeadingAlignInitializationPlugin as ManualHeadingAlignInitializationPlugin,
+)
+from .standard_plugins.preprocessor.StandardPreprocessorPlugin import (
+    StandardPreprocessorPlugin as StandardPreprocessorPlugin,
+)
+from .standard_plugins.StandardInertialPlugin import (
+    StandardInertialPlugin as StandardInertialPlugin,
+)
+from .standard_plugins.StandardLoggingPlugin import (
+    StandardLoggingPlugin as StandardLoggingPlugin,
+)
+from .standard_plugins.StandardOrchestrationPlugin import (
+    StandardOrchestrationPlugin as StandardOrchestrationPlugin,
+)
+from .standard_plugins.StandardRegistryPlugin import (
+    StandardRegistryPlugin as StandardRegistryPlugin,
+)
+from .standard_plugins.state_modeling.StandardStateModelingPlugin import (
+    StandardStateModelingPlugin as StandardStateModelingPlugin,
+)
+from .standard_plugins.StaticAlignInitializationPlugin import (
+    StaticAlignInitializationPlugin as StaticAlignInitializationPlugin,
+)
+from .tutorial_plugins.state_modeling.TutorialPosInsStateModelingPlugin import (
+    TutorialPosInsStateModelingPlugin as TutorialPosInsStateModelingPlugin,
+)
+from .tutorial_plugins.TutorialInitializationPlugin import (
+    TutorialInitializationPlugin as TutorialInitializationPlugin,
+)
+from .tutorial_plugins.TutorialLcmTransportPlugin import (
+    TutorialLcmTransportPlugin as TutorialLcmTransportPlugin,
+)
+from .tutorial_plugins.TutorialPosOrchestrationPlugin import (
+    TutorialPosOrchestrationPlugin as TutorialPosOrchestrationPlugin,
+)
+from .tutorial_plugins.TutorialPosVelOrchestrationPlugin import (
+    TutorialPosVelOrchestrationPlugin as TutorialPosVelOrchestrationPlugin,
+)
+from .tutorial_plugins.UiLogPlottingPlugin import (
+    UiLogPlottingPlugin as UiLogPlottingPlugin,
+)

pntos/cobra/advanced_plugins/Aspn23RosTransportPlugin.py

@@ -0,0 +1,149 @@
+from threading import Thread
+
+try:
+    import rclpy  # type: ignore[import-not-found]
+except ImportError as e:
+    raise ImportError(
+        'Is ROS installed and is its environment sourced? See the ROS usage '
+        'tutorial in the documentation.'
+    ) from e
+
+try:
+    from aspn23_ros_utils import AspnMsg, AspnRosNode  # type: ignore[import-not-found]
+except ImportError as e:
+    raise ImportError(
+        'Is the ASPN-ROS environment sourced? See the ROS usage tutorial in '
+        'the documentation.'
+    ) from e
+
+import contextlib
+
+from pntos.api import LoggingLevel, Mediator, Message, TransportPlugin
+from rclpy.executors import (  # type: ignore[import-not-found]
+    ExternalShutdownException,
+    SingleThreadedExecutor,
+)
+from rclpy.node import Subscription  # type: ignore[import-not-found]
+from rclpy.timer import Timer  # type: ignore[import-not-found]
+
+
+class Aspn23RosTransportPlugin(TransportPlugin):
+    """An example ROS Transport Plugin for ASPN23 implemented in Python"""
+
+    identifier: str
+    aspn_ros_node: AspnRosNode
+    mediator: Mediator
+    executor: SingleThreadedExecutor
+    thread: Thread
+
+    def __init__(self, identifier: str) -> None:
+        self.identifier = identifier
+        self._subs: list[Subscription] = []
+        self._topics: list[str] = []
+        self._scan_timer: Timer
+
+    def init_plugin(
+        self,
+        plugin_resources_location: str | None = None,
+        mediator: Mediator | None = None,
+    ) -> None:
+        """
+        PntOS plugin initialization function
+
+        This is called by the pntOS system before calling any other function.
+        """
+        if mediator is not None:
+            self.mediator = mediator
+        if not rclpy.ok():
+            rclpy.init()
+
+        self.executor = SingleThreadedExecutor()
+        self.aspn_ros_node = AspnRosNode('aspn23_ros_transport')
+        self.executor.add_node(self.aspn_ros_node)
+
+        def execute() -> None:
+            with contextlib.suppress(ExternalShutdownException):
+                self.executor.spin()
+
+        self.thread = Thread(target=execute)
+
+    def shutdown_plugin(self) -> None:
+        """
+        PntOS plugin shutdown function
+
+        This is called by the pntOS system when it is done with the plugin.
+        """
+        self.stop_listening()
+
+        self.executor.shutdown()
+        rclpy.try_shutdown()
+        self.thread.join()
+
+        self.aspn_ros_node.destroy_node()
+        self.mediator.log_message(
+            LoggingLevel.INFO, f'Shutdown plugin for {self.identifier}.'
+        )
+
+    def _scan_for_topics(self) -> None:
+        topic_info = self.aspn_ros_node.get_topic_names_and_types()
+        topics = [
+            topic
+            for topic, msg_types in topic_info
+            if 'aspn' in msg_types[0] and 'cobra' not in topic and 'pntos' not in topic
+        ]
+        for topic in topics:
+            if topic in self._topics:
+                continue
+            self._subs.append(
+                self.aspn_ros_node.subscribe_aspn(
+                    lambda msg, topic=topic: self.mediator.process_pntos_message(
+                        Message(msg, topic),
+                    ),
+                    topic,
+                )
+            )
+            self._topics.append(topic)
+            self.mediator.log_message(
+                LoggingLevel.DEBUG, f'Subscribed to ROS topic {topic}.'
+            )
+
+    def start_listening(self) -> None:
+        """Begin listening for ROS messages"""
+
+        self._scan_timer = self.aspn_ros_node.create_timer(0.01, self._scan_for_topics)
+        self.thread.start()
+        self.mediator.log_message(LoggingLevel.INFO, 'ROS transport started.')
+
+    def stop_listening(self) -> None:
+        """Shut down all ROS subscriptions belonging to this plugin"""
+        self._scan_timer.cancel()
+        for sub in self._subs:
+            self.aspn_ros_node.destroy_subscription(sub)
+        self._subs.clear()
+        self.mediator.log_message(LoggingLevel.INFO, 'ROS transport stopped.')
+
+    def broadcast_message(
+        self, message: Message, channel_name: str | None = None
+    ) -> None:
+        """Publish a ROS message"""
+        if not isinstance(message.wrapped_message, AspnMsg):
+            self.mediator.log_message(
+                LoggingLevel.ERROR,
+                f'Cannot publish message of type {type(message.wrapped_message)} on '
+                f'topic {channel_name}.',
+            )
+            return
+        if channel_name is None:
+            self.mediator.log_message(
+                LoggingLevel.WARN,
+                'No channel name specified (required in this implementation).',
+            )
+            return
+        if '-' in channel_name:
+            self.mediator.log_message(
+                LoggingLevel.WARN,
+                f'Channel name {channel_name} has dashes, which are not supported in '
+                f'ROS. Replacing with underscores.',
+            )
+            channel_name = channel_name.replace('-', '_')
+        self.aspn_ros_node.publish_aspn(message.wrapped_message, channel_name)

pntos/cobra/advanced_plugins/buscat/BuscatControllerPlugin.py

@@ -0,0 +1,261 @@
+import sys
+
+from pntos.api import (
+    CommonPlugin,
+    ControllerPlugin,
+    LoggingLevel,
+    LoggingPlugin,
+    Mediator,
+    RegistryPlugin,
+    TransportPlugin,
+    UiPlugin,
+)
+from pntos.cobra.config import BuscatConfig, config_from_registry
+from pntos.cobra.utils import (
+    SortedPlugins,
+    find_base_plugin_type,
+    print_message,
+    sort_plugins_dataclass,
+    validate_plugins,
+)
+
+from .BuscatMediator import BuscatMediator
+
+
+class BuscatControllerPlugin(ControllerPlugin):
+    """
+    This is a simple single-threaded Buscat controller plugin.
+
+    The purpose of this plugin is to route one or more data streams from
+    :class:`TransportPlugins <pntos.api.TransportPlugin>` back out through one or more
+    :class:`TransportPlugins <pntos.api.TransportPlugin>`, as specified in the supplied
+    :attr:`BuscatConfig.output_transports <pntos.cobra.config.BuscatConfig.output_transports>`.
+    This has the effect of combining multiple data streams and converting all input to the formats
+    supported by the output :class:`TransportPlugins <pntos.api.TransportPlugin>`. Note that this
+    controller does not do any buffering or sorting of the input data before publishing. It (or more
+    specifically, the :class:`BuscatMediator <pntos.cobra.internal.BuscatMediator>`) also does not
+    explicitly pass input data to plugins other than :class:`TransportPlugins <pntos.api.TransportPlugin>`,
+    and thus will not support sensor fusion without some modification.
+
+    Here are the plugins and corresponding expected number of instances this controller
+    looks for:
+
+    - LoggingPlugin - 1
+    - RegistryPlugin - 1
+    - TransportPlugin - at least 1
+    - UiPlugin - any, but only 1 can require the main thread
+
+    It checks for the expected plugins, sets up mediators, and then passes the mediators
+    to each plugin in :meth:`pntos.api.CommonPlugin.init_plugin`, then passes off to the
+    :meth:`_main` function.
+
+    Inside the main function, calls :meth:`pntos.api.TransportPlugin.start_listening` on
+    all transport plugins, checks if the UI needs to update (and passes it the thread if
+    so), then waits for a ctrl+c to exit pntOS.
+    """
+
+    _plugin_resources_location: str | None
+
+    def __init__(self, identifier: str) -> None:
+        """
+        Cobra Buscat Controller
+
+        Args:
+            identifier (str): The plugin identifier passed to the
+                :attr:`pntos.api.CommonPlugin.identifier` field.
+        """
+        self.identifier = identifier
+        self._plugins: list[CommonPlugin] = []
+        self._logging_plugin: LoggingPlugin | None = None
+        self._transport_plugins: list[TransportPlugin] = []
+        self._ui_plugins: list[UiPlugin] = []
+        self._registry_plugin: RegistryPlugin | None = None
+
+    def init_plugin(
+        self,
+        plugin_resources_location: str | None = None,
+        mediator: Mediator | None = None,
+    ) -> None:
+        if mediator is not None:
+            self._log(
+                LoggingLevel.ERROR, 'Controller plugin should not be passed a mediator.'
+            )
+        self._plugin_resources_location = plugin_resources_location
+
+    def shutdown_plugin(self) -> None:
+        self._log(LoggingLevel.INFO, 'Shutting down all plugins...')
+
+        for plugin in self._plugins:
+            # Don't shut down registry and logging plugins before the others
+            if not isinstance(plugin, (RegistryPlugin, LoggingPlugin)):
+                plugin.shutdown_plugin()
+
+        # Now close the registry and logging plugin last
+        if self._registry_plugin is not None:
+            self._registry_plugin.shutdown_plugin()
+        if self._logging_plugin is not None:
+            self._logging_plugin.shutdown_plugin()
+
+    identifier: str
+
+    def take_control(
+        self,
+        plugins: list[CommonPlugin],
+        plugin_resources_locations: list[str | None] | None = None,
+        initial_config: str | None = None,
+    ) -> None:
+        self._plugins = plugins
+
+        # Make sure there are enough plugins to run
+        (
+            self._registry_plugin,
+            self._logging_plugin,
+            self._transport_plugins,
+            self._ui_plugins,
+        ) = self._sort_and_validate_plugins(plugins)
+
+        # Hand mediators controller plugin so that mediators can call "shutdown_plugin"
+        # on error.
+        BuscatMediator._controller_plugin = self
+
+        # Create separate mediators to pass to each plugin
+        mediators = [
+            BuscatMediator(p.identifier, find_base_plugin_type(p))
+            for p in self._plugins
+        ]
+
+        # We need to make sure we got a valid plugin_resources_locations
+        if plugin_resources_locations is not None and len(
+            plugin_resources_locations
+        ) != len(self._plugins):
+            self._log(
+                LoggingLevel.ERROR,
+                'Length of plugin_resources_location '
+                + f'({len(plugin_resources_locations)}) does not equal the '
+                + f'number of plugins ({len(self._plugins)}). Passing '
+                + 'None to all plugins instead.',
+            )
+            plugin_resources_locations = None
+
+        # Initialize registry plugin first thing
+        reg_i = self._plugins.index(self._registry_plugin)
+        self._registry_plugin.init_plugin(
+            None
+            if plugin_resources_locations is None
+            else plugin_resources_locations[reg_i],
+            mediators[reg_i],
+        )
+
+        # Give mediators a registry
+        BuscatMediator.registry = self._registry_plugin.new_registry(initial_config)
+
+        # Initialize logger second
+        assert self._logging_plugin is not None
+        log_i = self._plugins.index(self._logging_plugin)
+        self._logging_plugin.init_plugin(
+            None
+            if plugin_resources_locations is None
+            else plugin_resources_locations[log_i],
+            mediators[log_i],
+        )
+
+        # Give mediators a logging plugin
+        BuscatMediator._logging_plugin = self._logging_plugin
+
+        # call init_plugin() on all the other plugins
+        for i, plugin in enumerate(self._plugins):
+            if i in (reg_i, log_i):
+                continue
+            plugin.init_plugin(
+                plugin_resources_location=None
+                if plugin_resources_locations is None
+                else plugin_resources_locations[i],
+                mediator=mediators[i],
+            )
+
+        # Give the mediators other needed plugins
+        BuscatMediator._transport_plugins = self._transport_plugins
+
+        # Set the mediators' output transport
+        temp_mediator = BuscatMediator(self.identifier, ControllerPlugin)
+        config = config_from_registry(BuscatConfig, temp_mediator, BuscatConfig.group)
+        if config is None:
+            self._log(
+                LoggingLevel.ERROR,
+                'Could not extract BuscatConfig from group "buscat". Cannot initialize Buscat controller plugin.',
+            )
+            return
+        BuscatMediator._output_transports = config.output_transports
+
+        # Pass off to main control loop
+        self._main()
+
+    def _sort_and_validate_plugins(
+        self, plugins: list[CommonPlugin]
+    ) -> tuple[RegistryPlugin, LoggingPlugin, list[TransportPlugin], list[UiPlugin]]:
+        """
+        Utility function to ensure ``plugins`` contains enough plugins to run pntOS
+        Cobra. Then assigns and dispatches them to the relevant fields on the
+        controller. Raises a :class:`RuntimeError` if plugins are
+        not as expected.
+        """
+        sorted_plugins: SortedPlugins = sort_plugins_dataclass(plugins)
+
+        if not validate_plugins(
+            sorted_plugins,
+            self._log,
+            registry_plugins=(1, 1),
+            logging_plugins=(1, 1),
+            transport_plugins=(1, 1000),
+        ):
+            raise RuntimeError('Not enough plugins to run pntOS.')
+
+        return (
+            sorted_plugins.registry_plugins[0],
+            sorted_plugins.logging_plugins[0],
+            sorted_plugins.transport_plugins,
+            sorted_plugins.ui_plugins,
+        )
+
+    def _log(self, level: LoggingLevel, message: str) -> None:
+        """
+        Utility logging method for controller.
+
+        NOTE: This is only intended for log messages originating from the
+        controller.
+        """
+        if self._logging_plugin is None:
+            print_message(level, ControllerPlugin.__name__, message)
+        else:
+            self._logging_plugin.log(ControllerPlugin, self.identifier, level, message)
+
+    def _main(self) -> None:
+        """
+        The main control of pntOS Cobra.
+        """
+        for transport in self._transport_plugins:
+            transport.start_listening()
+
+        # Run main thread from UI plugin, if needed
+        ui_needing_main_thrd = [p for p in self._ui_plugins if p.requires_main_thread()]
+        if len(ui_needing_main_thrd) > 1:
+            self._log(
+                LoggingLevel.ERROR,
+                f'Only 1 UiPlugin can require the main thread, but found {len(ui_needing_main_thrd)} needing the main thread. Cannot run pntOS.',
+            )
+        if ui_needing_main_thrd:
+            ui_needing_main_thrd[0].run_main_thread()
+
+        else:  # wait for ctrl + c to exit
+            self._log(
+                LoggingLevel.INFO,
+                'Press Ctrl + C at any time to shut down pntOS...',
+            )
+            try:
+                BuscatMediator._logging_error_event.wait()
+            except KeyboardInterrupt:
+                self._log(LoggingLevel.INFO, 'Keyboard Interrupt Detected.')
+
+        self.shutdown_plugin()
+        if BuscatMediator._logging_error_event.is_set():
+            sys.exit(1)

pntos/cobra/advanced_plugins/buscat/BuscatMediator.py

@@ -0,0 +1,113 @@
+from threading import Event
+from typing import ClassVar
+
+from aspn23 import TypeTimestamp
+from pntos.api import (
+    ControllerPlugin,
+    LoggingLevel,
+    LoggingPlugin,
+    Mediator,
+    Message,
+    PluginType,
+    Registry,
+    TransportPlugin,
+)
+from pntos.cobra.utils import print_message
+
+
+class BuscatMediator(Mediator):
+    """
+    This is a simple Buscat mediator implementation. It was designed to be used in conjunction with the
+    :class:`pntos.cobra.BuscatControllerPlugin` which is why this controller directly access private members.
+    It has one public member ``registry`` that other plugins are allowed to access.
+    """
+
+    _logging_plugin: LoggingPlugin | None = None
+    _transport_plugins: ClassVar[list[TransportPlugin]] = []
+    _controller_plugin: ControllerPlugin | None = None
+    _logging_error_event: Event = Event()
+    registry: Registry
+    _output_transports: tuple[str, ...]
+
+    def __init__(
+        self,
+        attached_plugin_identifier: str,
+        attached_plugin_type: PluginType,
+    ) -> None:
+        """
+        Buscat Mediator
+
+        Args:
+            attached_plugin_identifier (str): The identifier field of the plugin this
+                mediator is assigned to.
+            attached_plugin_type (PluginType | None): The abstract plugin type of the
+                plugin this mediator is assigned to.
+        """
+        self._attached_plugin_type: PluginType = attached_plugin_type
+        self._attached_plugin_identifier: str = attached_plugin_identifier
+
+    @property
+    def filter_description_list(self) -> list[str]:
+        return []
+
+    def request_solutions(
+        self,
+        solution_times: list[TypeTimestamp],
+        filter_description: str | None = None,
+    ) -> list[Message | None] | None:
+        pass
+
+    def process_pntos_message(self, message: Message) -> None:
+        # pass to designated transport plugin for broadcast
+        if message.source_identifier.startswith('/buscat/pntos'):
+            channel = message.source_identifier
+        else:
+            channel = '/buscat/pntos' + message.source_identifier
+
+        for output_transport in self._output_transports:
+            self.broadcast_aspn_message(
+                message,
+                transport=output_transport,
+                destination_identifier=channel,
+            )
+
+    def broadcast_aspn_message(
+        self,
+        message: Message,
+        transport: str | None = None,
+        destination_identifier: str | None = None,
+    ) -> None:
+        assert len(self._transport_plugins) != 0, (
+            'Transport plugin used before initialized and passed to mediator.'
+        )
+        sent = False
+        for transport_plugin in self._transport_plugins:
+            if transport is None or transport_plugin.identifier == transport:
+                transport_plugin.broadcast_message(message, destination_identifier)
+                sent = True
+        if not sent:
+            self._log_message(
+                LoggingLevel.WARN,
+                f'Transport "{transport}" not found. Unable to broadcast message.',
+            )
+
+    def log_message(self, level: LoggingLevel, message: str) -> None:
+        self._log_message(level, message, self._attached_plugin_type)
+
+    def _log_message(
+        self,
+        level: LoggingLevel,
+        message: str,
+        plugin_type: PluginType = ControllerPlugin,
+    ) -> None:
+        if self._logging_plugin is None:
+            print_message(level, plugin_type.__name__, message)
+        else:
+            self._logging_plugin.log(
+                plugin_type,
+                self._attached_plugin_identifier,
+                level,
+                message,
+            )
+        if level is LoggingLevel.ERROR and self._controller_plugin is not None:
+            self._logging_error_event.set()