From 8cb5224e58a43df15c898253d49fe257a9c7e785 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 7 Aug 2023 00:25:58 -0700 Subject: [PATCH] Started arming behaviors --- .dockerignore | 2 + angler_behaviors/behavior_tree/__init__.py | 1 + .../blocks}/__init__.py | 0 .../behavior_tree/blocks/arming.py | 112 +++++++ .../behavior_tree/blocks/planning.py | 19 ++ .../behavior_tree/blocks/trajectory.py | 19 ++ .../behavior_tree/components/__init__.py | 0 .../components/service_clients.py | 279 ++++++++++++++++++ .../behavior_tree/context/__init__.py | 35 +++ angler_behaviors/behavior_tree/tree.py | 50 ++++ .../tpik/controller.py | 2 +- angler_utils/scripts/arm.sh | 2 +- angler_utils/scripts/disarm.sh | 2 +- 13 files changed, 520 insertions(+), 3 deletions(-) create mode 100644 angler_behaviors/behavior_tree/__init__.py rename angler_behaviors/{angler_behaviors => behavior_tree/blocks}/__init__.py (100%) create mode 100644 angler_behaviors/behavior_tree/blocks/arming.py create mode 100644 angler_behaviors/behavior_tree/blocks/planning.py create mode 100644 angler_behaviors/behavior_tree/blocks/trajectory.py create mode 100644 angler_behaviors/behavior_tree/components/__init__.py create mode 100644 angler_behaviors/behavior_tree/components/service_clients.py create mode 100644 angler_behaviors/behavior_tree/context/__init__.py create mode 100644 angler_behaviors/behavior_tree/tree.py diff --git a/.dockerignore b/.dockerignore index d578fc9..888c12c 100644 --- a/.dockerignore +++ b/.dockerignore @@ -8,6 +8,8 @@ !angler_planning !angler_mux !angler_utils +!angler_kinematics +!angler_behaviors !angler.repos !sim.repos !.docker/entrypoints diff --git a/angler_behaviors/behavior_tree/__init__.py b/angler_behaviors/behavior_tree/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/angler_behaviors/behavior_tree/__init__.py @@ -0,0 +1 @@ + diff --git a/angler_behaviors/angler_behaviors/__init__.py b/angler_behaviors/behavior_tree/blocks/__init__.py similarity index 100% rename from angler_behaviors/angler_behaviors/__init__.py rename to angler_behaviors/behavior_tree/blocks/__init__.py diff --git a/angler_behaviors/behavior_tree/blocks/arming.py b/angler_behaviors/behavior_tree/blocks/arming.py new file mode 100644 index 0000000..d76c5ef --- /dev/null +++ b/angler_behaviors/behavior_tree/blocks/arming.py @@ -0,0 +1,112 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import behavior_tree.context as context +import py_trees +import py_trees_ros +from components.service_clients import FromConstant as ServiceClientFromConstant +from std_msgs.msg import Bool +from std_srvs.srv import SetBool + + +def make_save_armed_behavior() -> py_trees.behaviour.Behaviour: + """Save a command to arm/disarm the system to the blackboard. + + Returns: + A ToBlackboard behavior which saves commands to arm/disarm the system to the + blackboard. + """ + return py_trees_ros.subscribers.ToBlackboard( + name="ROS2BB: Arm", + topic_name=context.TOPIC_ARM, + topic_type=Bool, + qos_profile=py_trees_ros.utilities.qos_profile_latched(), + blackboard_variables={context.BB_ARMED: None}, + clearing_policy=py_trees.common.ClearingPolicy.NEVER, + ) + + +def make_block_on_disarm_behavior( + on_disarm_behavior: py_trees.behaviour.Behaviour, + tasks: py_trees.behaviour.Behaviour, +) -> py_trees.behaviour.Behaviour: + """Make a behavior that blocks when the system is disarmed. + + Args: + on_disarm_behavior: The behavior to run when a disarm is triggered. + tasks: A behavior with the tasks to run. + + Returns: + A Selector behavior with the disarm EternalGuard as the highest priority + behavior and the provided tasks as second priority. + """ + + def check_disarm_on_blackboard( + blackboard: py_trees.blackboard.Blackboard, + ) -> bool: + return blackboard.disarm # type: ignore + + disarm = py_trees.decorators.EternalGuard( + name="Disarm?", + condition=check_disarm_on_blackboard, + blackboard_keys={"disarm"}, + child=on_disarm_behavior, + ) + + return py_trees.composites.Selector( + name="Block tasks on disarm", memory=False, children=[disarm, tasks] + ) + + +def make_arming_behavior( + arm: bool, post_arming_behavior: py_trees.behaviour.Behaviour | None +): + set_passthrough_mode = ServiceClientFromConstant( + name=f"Enable PWM passthrough mode: {arm}", + service_type=SetBool, + service_name=context.SRV_ENABLE_PASSTHROUGH, + service_request=Bool(data=arm), + key_response=context.BB_PASSTHROUGH_REQUEST_RESPONSE, + ) + + arm_blue_controller = ServiceClientFromConstant( + name=f"Arm Blue controller: {arm}", + service_type=SetBool, + service_name=context.SRV_ARM_BLUE, + service_request=Bool(data=arm), + key_response=context.BB_BLUE_ARMING_REQUEST_RESPONSE, + ) + + arm_angler_controller = ServiceClientFromConstant( + name=f"Arm Angler controller: {arm}", + service_type=SetBool, + service_name=context.SRV_ARM_ANGLER, + service_request=Bool(data=arm), + key_response=context.BB_ANGLER_ARMING_REQUEST_RESPONSE, + ) + + # TODO(evan): Save the result / add a post arming behavior + # TODO(evan): Wait for the result of each service call to be true + + return py_trees.composites.Sequence( + name="Arm system" if arm else "Disarm system", + memory=True, + children=[set_passthrough_mode, arm_blue_controller, arm_angler_controller], + ) diff --git a/angler_behaviors/behavior_tree/blocks/planning.py b/angler_behaviors/behavior_tree/blocks/planning.py new file mode 100644 index 0000000..41365e2 --- /dev/null +++ b/angler_behaviors/behavior_tree/blocks/planning.py @@ -0,0 +1,19 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. diff --git a/angler_behaviors/behavior_tree/blocks/trajectory.py b/angler_behaviors/behavior_tree/blocks/trajectory.py new file mode 100644 index 0000000..41365e2 --- /dev/null +++ b/angler_behaviors/behavior_tree/blocks/trajectory.py @@ -0,0 +1,19 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. diff --git a/angler_behaviors/behavior_tree/components/__init__.py b/angler_behaviors/behavior_tree/components/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/angler_behaviors/behavior_tree/components/service_clients.py b/angler_behaviors/behavior_tree/components/service_clients.py new file mode 100644 index 0000000..d787792 --- /dev/null +++ b/angler_behaviors/behavior_tree/components/service_clients.py @@ -0,0 +1,279 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# License: BSD +# https://raw.github.com/splintered-reality/py_trees_ros/license/LICENSE +# +############################################################################## +# Documentation +############################################################################## + +""" +Behaviours for ROS services. + +Note from Evan: This is a direct port from this PR: +https://github.com/splintered-reality/py_trees_ros/pull/215 +I want to use this, but don't feel like building yet another package +from source. This will be removed when the PR is merged. +""" + +############################################################################## +# Imports +############################################################################## + +import typing +import uuid +from asyncio.tasks import wait_for + +import py_trees +from py_trees_ros import exceptions + +############################################################################## +# Behaviours +############################################################################## + + +class FromBlackboard(py_trees.behaviour.Behaviour): + """ + An service client interface that draws requests from the blackboard. The + lifecycle of this behaviour works as follows: + + * :meth:`initialise`: check blackboard for a request and send + * :meth:`update`: if a request was sent, monitor progress + * :meth:`terminate`: if interrupted while running, send a cancel request + + As a consequence, the status of this behaviour can be interpreted as follows: + + * :data:`~py_trees.common.Status.FAILURE`: no request was found to send, + the server was not ready, or it failed while executing + * :data:`~py_trees.common.Status.RUNNING`: a request was sent and is still + executing + * :data:`~py_trees.common.Status.SUCCESS`: sent request has completed with success + + To block on the arrival of a request on the blackboard, use with the + :class:`py_trees.behaviours.WaitForBlackboardVariable` behaviour. e.g. + + Args: + name: name of the behaviour + service_type: spec type for the service + service_name: where you can find the service + key_request: name of the key for the request on the blackboard + key_response: optional name of the key for the response on the blackboard (default: None) + wait_for_server_timeout_sec: use negative values for a blocking but periodic check (default: -3.0) + + .. note:: + The default setting for timeouts (a negative value) will suit + most use cases. With this setting the behaviour will periodically check and + issue a warning if the server can't be found. Actually aborting the setup can + usually be left up to the behaviour tree manager. + """ + + def __init__( + self, + name: str, + service_type: typing.Any, + service_name: str, + key_request: str, + key_response: typing.Optional[str] = None, + wait_for_server_timeout_sec: float = -3.0, + ): + super().__init__(name) + self.service_type = service_type + self.service_name = service_name + self.wait_for_server_timeout_sec = wait_for_server_timeout_sec + self.blackboard = self.attach_blackboard_client(name=self.name) + self.blackboard.register_key( + key="request", + access=py_trees.common.Access.READ, + # make sure to namespace it if not already + remap_to=py_trees.blackboard.Blackboard.absolute_name("/", key_request), + ) + self.write_response_to_blackboard = key_response is not None + if self.write_response_to_blackboard: + self.blackboard.register_key( + key="response", + access=py_trees.common.Access.WRITE, + # make sure to namespace it if not already + remap_to=py_trees.blackboard.Blackboard.absolute_name( + "/", key_response + ), + ) + + self.node = None + self.service_client = None + + def setup(self, **kwargs): + """ + Setup the service client and ensure it is available. + + Args: + **kwargs (:obj:`dict`): distribute arguments to this + behaviour and in turn, all of it's children + + Raises: + :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs + :class:`~py_trees_ros.exceptions.TimedOutError`: if the service server could not be found + """ + self.logger.debug("{}.setup()".format(self.qualified_name)) + try: + self.node = kwargs["node"] + except KeyError as e: + error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format( + self.qualified_name + ) + raise KeyError(error_message) from e # 'direct cause' traceability + + self.service_client = self.node.create_client( + srv_type=self.service_type, srv_name=self.service_name + ) + + result = None + if self.wait_for_server_timeout_sec > 0.0: + result = self.service_client.wait_for_service( + timeout_sec=self.wait_for_server_timeout_sec + ) + else: + iterations = 0 + period_sec = -1.0 * self.wait_for_server_timeout_sec + while not result: + iterations += 1 + result = self.service_client.wait_for_service(timeout_sec=period_sec) + if not result: + self.node.get_logger().warning( + "waiting for service server ... [{}s][{}][{}]".format( + iterations * period_sec, + self.service_name, + self.qualified_name, + ) + ) + if not result: + self.feedback_message = "timed out waiting for the server [{}]".format( + self.service_name + ) + self.node.get_logger().error( + "{}[{}]".format(self.feedback_message, self.qualified_name) + ) + raise exceptions.TimedOutError(self.feedback_message) + else: + self.feedback_message = "... connected to service server [{}]".format( + self.service_name + ) + self.node.get_logger().info( + "{}[{}]".format(self.feedback_message, self.qualified_name) + ) + + def initialise(self): + """ + Reset the internal variables and kick off a new request request. + """ + self.logger.debug("{}.initialise()".format(self.qualified_name)) + + # initialise some temporary variables + self.service_future = None + + try: + if self.service_client.service_is_ready(): + self.service_future = self.service_client.call_async( + self.blackboard.request + ) + except (KeyError, TypeError): + pass # self.service_future will be None, check on that + + def update(self): + """ + Check only to see whether the underlying service server has + succeeded, is running, or has cancelled/aborted for some reason and + map these to the usual behaviour return states. + + Returns: + :class:`py_trees.common.Status` + """ + self.logger.debug("{}.update()".format(self.qualified_name)) + + if self.service_future is None: + # either there was no request on the blackboard, the request's type + # was wrong, or the service server wasn't ready + return py_trees.common.Status.FAILURE + elif not self.service_future.done(): + # service has been called but hasn't yet returned a result + return py_trees.common.Status.RUNNING + else: + # service has succeeded; get the result + self.response = self.service_future.result() + if self.write_response_to_blackboard: + self.blackboard.response = self.response + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status: py_trees.common.Status): + """ + If running and the current request has not already succeeded, cancel it. + + Args: + new_status: the behaviour is transitioning to this new status + """ + self.logger.debug( + "{}.terminate({})".format( + self.qualified_name, + "{}->{}".format(self.status, new_status) + if self.status != new_status + else "{}".format(new_status), + ) + ) + if (self.service_future is not None) and (not self.service_future.done()): + self.service_client.remove_pending_request(self.service_future) + + def shutdown(self): + """ + Clean up the service client when shutting down. + """ + self.service_client.destroy() + + +class FromConstant(FromBlackboard): + """ + Convenience version of the service client that only ever sends the + same goal. + + .. see-also: :class:`py_trees_ros.service_clients.FromBlackboard` + + Args: + name: name of the behaviour + name: name of the behaviour + service_type: spec type for the service + service_name: where you can find the service + service_request: the request to send + key_response: optional name of the key for the response on the blackboard (default: None) + wait_for_server_timeout_sec: use negative values for a blocking but periodic check (default: -3.0) + + .. note:: + The default setting for timeouts (a negative value) will suit + most use cases. With this setting the behaviour will periodically check and + issue a warning if the server can't be found. Actually aborting the setup can + usually be left up to the behaviour tree manager. + """ + + def __init__( + self, + name: str, + service_type: typing.Any, + service_name: str, + service_request: typing.Any, + key_response: typing.Optional[str] = None, + wait_for_server_timeout_sec: float = -3.0, + ): + unique_id = uuid.uuid4() + key_request = "/goal_" + str(unique_id) + super().__init__( + service_type=service_type, + service_name=service_name, + key_request=key_request, + key_response=key_response, + name=name, + wait_for_server_timeout_sec=wait_for_server_timeout_sec, + ) + # parent already instantiated a blackboard client + self.blackboard.register_key( + key=key_request, + access=py_trees.common.Access.WRITE, + ) + self.blackboard.set(name=key_request, value=service_request) diff --git a/angler_behaviors/behavior_tree/context/__init__.py b/angler_behaviors/behavior_tree/context/__init__.py new file mode 100644 index 0000000..9b9934e --- /dev/null +++ b/angler_behaviors/behavior_tree/context/__init__.py @@ -0,0 +1,35 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +ANGLER_NAMESPACE = "/angler" + +# Topic names +TOPIC_ARM = f"{ANGLER_NAMESPACE}/arm" + +# Blackboard keys +BB_ARMED = "armed" +BB_PASSTHROUGH_REQUEST_RESPONSE = "passthrough_request_response" +BB_BLUE_ARMING_REQUEST_RESPONSE = "blue_arming_request_response" +BB_ANGLER_ARMING_REQUEST_RESPONSE = "angler_arming_request_response" + +# Service names +SRV_ENABLE_PASSTHROUGH = "/blue/cmd/enable_passthrough" +SRV_ARM_BLUE = "/blue/cmd/arm" +SRV_ARM_ANGLER = "/angler/cmd/arm" diff --git a/angler_behaviors/behavior_tree/tree.py b/angler_behaviors/behavior_tree/tree.py new file mode 100644 index 0000000..6106f7d --- /dev/null +++ b/angler_behaviors/behavior_tree/tree.py @@ -0,0 +1,50 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import py_trees +import py_trees_ros +import rclpy + + +def make_angler_tree() -> py_trees.behaviour.Behaviour: + root = py_trees.composites.Parallel( + name="Angler Autonomy", + policy=py_trees.common.ParallelPolicy.SuccessOnAll(synchronise=False), + ) + + return root + + +def main(args: list[str] | None = None): + """Run the pre-planned waypoint planner.""" + rclpy.init(args=args) + + # Create the behavior tree + root = make_angler_tree() + tree = py_trees_ros.trees.BehaviourTree(root) + + # Setup the tree; this will throw if there is a timeout + tree.setup(timeout=5.0) + + # Run the tree at a rate of 20hz + tree.tick_tock(50) + + tree.shutdown() + rclpy.shutdown() diff --git a/angler_control/inverse_kinematic_controllers/tpik/controller.py b/angler_control/inverse_kinematic_controllers/tpik/controller.py index 3037254..4e8c501 100644 --- a/angler_control/inverse_kinematic_controllers/tpik/controller.py +++ b/angler_control/inverse_kinematic_controllers/tpik/controller.py @@ -169,7 +169,7 @@ def __init__(self) -> None: # Services self.arm_controller_srv = self.create_service( - SetBool, "/angler/tpik/arm", self.arm_controller_cb + SetBool, "/angler/cmd/arm", self.arm_controller_cb ) # Create a new callback group for the control loop timer diff --git a/angler_utils/scripts/arm.sh b/angler_utils/scripts/arm.sh index 2981afd..8f146c2 100755 --- a/angler_utils/scripts/arm.sh +++ b/angler_utils/scripts/arm.sh @@ -6,4 +6,4 @@ ros2 service call /blue/cmd/enable_passthrough std_srvs/srv/SetBool data:\ true\ ros2 service call /blue/cmd/arm std_srvs/srv/SetBool data:\ true\ -ros2 service call /angler/tpik/arm std_srvs/srv/SetBool data:\ true\ +ros2 service call /angler/cmd/arm std_srvs/srv/SetBool data:\ true\ diff --git a/angler_utils/scripts/disarm.sh b/angler_utils/scripts/disarm.sh index 16a4e0f..c9bba3e 100755 --- a/angler_utils/scripts/disarm.sh +++ b/angler_utils/scripts/disarm.sh @@ -6,4 +6,4 @@ ros2 service call /blue/cmd/enable_passthrough std_srvs/srv/SetBool data:\ false ros2 service call /blue/cmd/arm std_srvs/srv/SetBool data:\ false\ -ros2 service call /angler/tpik/arm std_srvs/srv/SetBool data:\ false\ +ros2 service call /angler/cmd/arm std_srvs/srv/SetBool data:\ false\