add ros2 lifecycle (#97)

* add ros2 lifecycle

* remove debug print

ros2lifecycle/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>ros2lifecycle</name>
+  <version>0.4.0</version>
+  <description>
+    The lifecycle command for ROS 2 command line tools.
+  </description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>Apache License 2.0</license>
+
+  <depend>rcl_interfaces</depend>
+  <depend>rclpy</depend>
+  <depend>ros2cli</depend>
+
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>ros2node</exec_depend>
+  <exec_depend>ros2service</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros2lifecycle/ros2lifecycle/api/__init__.py

@@ -0,0 +1,168 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rclpy
+
+from lifecycle_msgs.srv import ChangeState
+from lifecycle_msgs.srv import GetAvailableTransitions
+from lifecycle_msgs.srv import GetState
+from ros2node.api import get_node_names as get_all_node_names
+from ros2service.api import get_service_names_and_types
+
+
+def get_node_names(*, node, include_hidden_nodes=False):
+    node_names = get_all_node_names(
+        node=node, include_hidden_nodes=include_hidden_nodes)
+    service_names_and_types = get_service_names_and_types(node=node)
+    return [
+        n for n in node_names
+        if _has_lifecycle(n, service_names_and_types)]
+
+
+def _has_lifecycle(node_name, service_names_and_types):
+    for (service_name, service_types) in service_names_and_types:
+        if (
+            service_name == '/{node_name}/get_state'.format_map(locals()) and
+            'lifecycle_msgs/GetState' in service_types
+        ):
+            return True
+    return False
+
+
+def call_get_states(*, node, node_names):
+    clients = {}
+    futures = {}
+    # create clients
+    for node_name in node_names:
+        client = node.create_client(
+            GetState,
+            '/{node_name}/get_state'.format_map(locals()))
+        clients[node_name] = client
+
+    # wait until all clients have been called
+    while True:
+        for node_name in [n for n in node_names if n not in futures]:
+            # call as soon as ready
+            client = clients[node_name]
+            if client.service_is_ready():
+                request = GetState.Request()
+                request.node_name = node_name
+                future = client.call_async(request)
+                futures[node_name] = future
+
+        if len(futures) == len(clients):
+            break
+        rclpy.spin_once(node, timeout_sec=1.0)
+
+    # wait for all responses
+    for future in futures.values():
+        rclpy.spin_until_future_complete(node, future)
+
+    # return current state or exception for each node
+    states = {}
+    for node_name, future in futures.items():
+        if future.result() is not None:
+            response = future.result()
+            states[node_name] = response.current_state
+        else:
+            states[node_name] = future.exception()
+    return states
+
+
+def call_get_available_transitions(*, node, states):
+    clients = {}
+    futures = {}
+    # create clients
+    for node_name in states.keys():
+        client = node.create_client(
+            GetAvailableTransitions,
+            '/{node_name}/get_available_transitions'.format_map(locals()))
+        clients[node_name] = client
+
+    # wait until all clients have been called
+    while True:
+        for node_name in [n for n in states.keys() if n not in futures]:
+            # call as soon as ready
+            client = clients[node_name]
+            if client.service_is_ready():
+                request = GetAvailableTransitions.Request()
+                request.node_name = node_name
+                future = client.call_async(request)
+                futures[node_name] = future
+
+        if len(futures) == len(clients):
+            break
+        rclpy.spin_once(node, timeout_sec=1.0)
+
+    # wait for all responses
+    for future in futures.values():
+        rclpy.spin_until_future_complete(node, future)
+
+    # return transitions from current state or exception for each node
+    transitions = {}
+    for node_name, future in futures.items():
+        if future.result() is not None:
+            response = future.result()
+            transitions[node_name] = []
+            for transition_description in response.available_transitions:
+                if (
+                    states[node_name] is None or
+                    transition_description.start_state == states[node_name]
+                ):
+                    transitions[node_name].append(
+                        transition_description.transition)
+        else:
+            transitions[node_name] = future.exception()
+    return transitions
+
+
+def call_change_states(*, node, transitions):
+    clients = {}
+    futures = {}
+    # create clients
+    for node_name in transitions.keys():
+        client = node.create_client(
+            ChangeState,
+            '/{node_name}/change_state'.format_map(locals()))
+        clients[node_name] = client
+
+    # wait until all clients have been called
+    while True:
+        for node_name in [n for n in transitions.keys() if n not in futures]:
+            # call as soon as ready
+            client = clients[node_name]
+            if client.service_is_ready():
+                request = ChangeState.Request()
+                request.node_name = node_name
+                request.transition = transitions[node_name]
+                future = client.call_async(request)
+                futures[node_name] = future
+
+        if len(futures) == len(clients):
+            break
+        rclpy.spin_once(node, timeout_sec=1.0)
+
+    # wait for all responses
+    for future in futures.values():
+        rclpy.spin_until_future_complete(node, future)
+
+    # return success flag or exception for each node
+    results = {}
+    for node_name, future in futures.items():
+        if future.result() is not None:
+            response = future.result()
+            results[node_name] = response.success
+        else:
+            results[node_name] = future.exception()
+    return results

ros2lifecycle/ros2lifecycle/command/lifecycle.py

@@ -0,0 +1,40 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ros2cli.command import add_subparsers
+from ros2cli.command import CommandExtension
+from ros2cli.verb import get_verb_extensions
+
+
+class LifecycleCommand(CommandExtension):
+    """Various lifecycle related sub-commands."""
+
+    def add_arguments(self, parser, cli_name):
+        self._subparser = parser
+
+        # get verb extensions and let them add their arguments
+        verb_extensions = get_verb_extensions('ros2lifecycle.verb')
+        add_subparsers(
+            parser, cli_name, '_verb', verb_extensions, required=False)
+
+    def main(self, *, parser, args):
+        if not hasattr(args, '_verb'):
+            # in case no verb was passed
+            self._subparser.print_help()
+            return 0
+
+        extension = getattr(args, '_verb')
+
+        # call the verb's main method
+        return extension.main(args=args)

ros2lifecycle/ros2lifecycle/verb/__init__.py

@@ -0,0 +1,44 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ros2cli.plugin_system import PLUGIN_SYSTEM_VERSION
+from ros2cli.plugin_system import satisfies_version
+
+
+class VerbExtension:
+    """
+    The extension point for 'lifecycle' verb extensions.
+
+    The following properties must be defined:
+    * `NAME` (will be set to the entry point name)
+
+    The following methods must be defined:
+    * `main`
+
+    The following methods can be defined:
+    * `add_arguments`
+    """
+
+    NAME = None
+    EXTENSION_POINT_VERSION = '0.1'
+
+    def __init__(self):
+        super(VerbExtension, self).__init__()
+        satisfies_version(PLUGIN_SYSTEM_VERSION, '^0.1')
+
+    def add_arguments(self, parser, cli_name):
+        pass
+
+    def main(self, *, args):
+        raise NotImplementedError()

ros2lifecycle/ros2lifecycle/verb/get.py

@@ -0,0 +1,95 @@
+# Copyright 2018 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import sys
+
+from ros2cli.node.direct import DirectNode
+from ros2cli.node.strategy import add_arguments
+from ros2cli.node.strategy import NodeStrategy
+from ros2lifecycle.api import call_get_available_transitions
+from ros2lifecycle.api import call_get_states
+from ros2lifecycle.api import get_node_names
+from ros2lifecycle.verb import VerbExtension
+from ros2node.api import NodeNameCompleter
+
+
+class GetVerb(VerbExtension):
+    """Get lifecycle state."""
+
+    def add_arguments(self, parser, cli_name):  # noqa: D102
+        add_arguments(parser)
+        arg = parser.add_argument(
+            'node_name', nargs='?', help='Name of the ROS node')
+        arg.completer = NodeNameCompleter(
+            include_hidden_nodes_key='include_hidden_nodes')
+        parser.add_argument(
+            '--include-hidden-nodes', action='store_true',
+            help='Consider hidden nodes as well')
+        parser.add_argument(
+            '--transitions', action='store_true',
+            help='Output possible transitions')
+
+    def main(self, *, args):  # noqa: D102
+        with NodeStrategy(args) as node:
+            node_names = get_node_names(
+                node=node, include_hidden_nodes=args.include_hidden_nodes)
+
+        if args.node_name:
+            if args.node_name not in node_names:
+                return 'Node not found'
+            node_names = [args.node_name]
+
+        with DirectNode(args) as node:
+            states = call_get_states(node=node, node_names=node_names)
+
+            # output exceptions
+            for node_name in sorted(states.keys()):
+                state = states[node_name]
+                if isinstance(state, Exception):
+                    print(
+                        'Exception while calling service of node '
+                        "'{node_name}': {state}".format_map(locals()),
+                        file=sys.stderr)
+                    del states[node_name]
+                    if args.node_name:
+                        return 1
+
+            if args.transitions:
+                transitions = call_get_available_transitions(
+                    node=node, states=states)
+
+            # output current states
+            for node_name in sorted(states.keys()):
+                state = states[node_name]
+                prefix = ''
+                if not args.node_name:
+                    prefix = '{node_name}: '.format_map(locals())
+                print(
+                    prefix + '{state.label} [{state.id}]'.format_map(locals()))
+
+                if args.transitions:
+                    if isinstance(transitions[node_name], Exception):
+                        print(
+                            'Exception while calling service of node '
+                            "'{node_name}': {transitions[node_name]}"
+                            .format_map(locals()), file=sys.stderr)
+                        if args.node_name:
+                            return 1
+                    elif transitions[node_name]:
+                        for transition in transitions[node_name]:
+                            print(
+                                '-> {transition.label} [{transition.id}]'
+                                .format_map(locals()))
+                    else:
+                        print('  no transitions available')