From 0a590373bf143390fff0754eb0369dd02ae0777d Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Thu, 11 May 2023 15:44:28 -0600 Subject: [PATCH 01/18] Added the example call_do_objective scripts --- src/moveit_studio_agent_utils/CMakeLists.txt | 36 +++++++++++++ src/moveit_studio_agent_utils/README.md | 5 ++ src/moveit_studio_agent_utils/package.xml | 32 +++++++++++ .../scripts/__init__.py | 0 .../scripts/call_do_objective.py | 46 ++++++++++++++++ .../scripts/call_do_objective_waypoint.py | 54 +++++++++++++++++++ 6 files changed, 173 insertions(+) create mode 100644 src/moveit_studio_agent_utils/CMakeLists.txt create mode 100644 src/moveit_studio_agent_utils/README.md create mode 100644 src/moveit_studio_agent_utils/package.xml create mode 100755 src/moveit_studio_agent_utils/scripts/__init__.py create mode 100755 src/moveit_studio_agent_utils/scripts/call_do_objective.py create mode 100755 src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py diff --git a/src/moveit_studio_agent_utils/CMakeLists.txt b/src/moveit_studio_agent_utils/CMakeLists.txt new file mode 100644 index 00000000..c9498c26 --- /dev/null +++ b/src/moveit_studio_agent_utils/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.16.3) +project(moveit_studio_agent_utils) + +find_package(ament_cmake REQUIRED) +find_package(moveit_studio_agent_msgs REQUIRED) +find_package(moveit_studio_behavior_interface REQUIRED) +find_package(moveit_studio_behavior_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(std_msgs REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + moveit_studio_agent_msgs + moveit_studio_behavior_interface + moveit_studio_behavior_msgs + std_msgs +) + +############# +## Install ## +############# + +# Install scripts directory +install(PROGRAMS + scripts/call_do_objective.py + scripts/call_do_objective_waypoint.py + DESTINATION lib/${PROJECT_NAME} +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/moveit_studio_agent_utils/README.md b/src/moveit_studio_agent_utils/README.md new file mode 100644 index 00000000..d2adfcc1 --- /dev/null +++ b/src/moveit_studio_agent_utils/README.md @@ -0,0 +1,5 @@ +# moveit_studio_agent_utils + +Provides Scripts to interact with MoveIt Studio Agent API programatically. + +Please see the [Interact with the Objective Server Directly](https://docs.picknik.ai/en/stable/how_to/interact_with_the_objective_server_directly/interact_with_the_objective_server_directly.html) tutorial for more information on these scripts and their use. diff --git a/src/moveit_studio_agent_utils/package.xml b/src/moveit_studio_agent_utils/package.xml new file mode 100644 index 00000000..7b643905 --- /dev/null +++ b/src/moveit_studio_agent_utils/package.xml @@ -0,0 +1,32 @@ + + + moveit_studio_agent_utils + 2.1.0 + Package containing scripts for interacting with MoveIt Studio Agent + + Chance Cardona + Chance Cardona + + BSD-3 + + ament_cmake + + moveit_common + + moveit_studio_agent_msgs + moveit_studio_behavior_interface + moveit_studio_behavior_msgs + std_msgs + + ament_lint_auto + ament_cmake_gtest + ament_clang_format + ament_clang_tidy + ament_cmake_copyright + ament_cmake_lint_cmake + picknik_ament_copyright + + + ament_cmake + + diff --git a/src/moveit_studio_agent_utils/scripts/__init__.py b/src/moveit_studio_agent_utils/scripts/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective.py b/src/moveit_studio_agent_utils/scripts/call_do_objective.py new file mode 100755 index 00000000..4bd3500f --- /dev/null +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 + +# Copyright 2022 PickNik Inc. +# All rights reserved. +# +# Unauthorized copying of this code base via any medium is strictly prohibited. +# Proprietary and confidential. + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +import sys + +from moveit_studio_agent_msgs.action import DoObjectiveSequence + + +class DoObjectiveSequenceClient(Node): + def __init__(self): + super().__init__("DoObjectiveSequence") + self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") + + def send_goal(self, objective_name): + goal_msg = DoObjectiveSequence.Goal() + goal_msg.objective_name = objective_name + self._action_client.wait_for_server() + result = self._action_client.send_goal_async(goal_msg) + return result + + +def main(args=None): + if len(sys.argv) < 2: + print( + "usage: ros2 run moveit_studio_behavior call_do_objective.py 'Objective Name'" + ) + else: + rclpy.init(args=args) + + client = DoObjectiveSequenceClient() + + future = client.send_goal(sys.argv[1]) + + rclpy.spin_until_future_complete(client, future) + + +if __name__ == "__main__": + main() diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py new file mode 100755 index 00000000..d633f30c --- /dev/null +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 + +# Copyright 2022 PickNik Inc. +# All rights reserved. +# +# Unauthorized copying of this code base via any medium is strictly prohibited. +# Proprietary and confidential. + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +import sys + +from moveit_studio_agent_msgs.action import DoObjectiveSequence +from moveit_studio_behavior_msgs.msg import BehaviorParameter, BehaviorParameterDescription + +class DoObjectiveSequenceClient(Node): + def __init__(self): + super().__init__("DoObjectiveSequence") + self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") + + def send_goal(self, objective_name, waypoint_name="Behind"): + goal_msg = DoObjectiveSequence.Goal() + goal_msg.objective_name = objective_name + + behavior_parameter = BehaviorParameter() + behavior_parameter.behavior_namespaces.append("move_to_joint_state") + behavior_parameter.description.name = "waypoint_name" + behavior_parameter.description.type = BehaviorParameterDescription.TYPE_STRING + behavior_parameter.string_value = waypoint_name + goal_msg.parameter_overrides = [behavior_parameter] + + self._action_client.wait_for_server() + result = self._action_client.send_goal_async(goal_msg) + return result + + +def main(args=None): + if len(sys.argv) < 2: + print( + "usage: ros2 run moveit_studio_behavior call_do_objective.py 'Objective Name'" + ) + else: + rclpy.init(args=args) + + client = DoObjectiveSequenceClient() + + future = client.send_goal(sys.argv[1], sys.argv[2]) + + rclpy.spin_until_future_complete(client, future) + + +if __name__ == "__main__": + main() From f12618778178a41ffaba2ff6789d7601dd7888c8 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Thu, 11 May 2023 16:09:23 -0600 Subject: [PATCH 02/18] Updated call_do_objective_waypoint to be consistent --- .../scripts/call_do_objective_waypoint.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py index d633f30c..70a3481d 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py @@ -12,16 +12,19 @@ import sys from moveit_studio_agent_msgs.action import DoObjectiveSequence -from moveit_studio_behavior_msgs.msg import BehaviorParameter, BehaviorParameterDescription +from moveit_studio_behavior_msgs.msg import ( + BehaviorParameter, + BehaviorParameterDescription, +) class DoObjectiveSequenceClient(Node): def __init__(self): super().__init__("DoObjectiveSequence") self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") - def send_goal(self, objective_name, waypoint_name="Behind"): + def send_goal(self, waypoint_name="Behind"): goal_msg = DoObjectiveSequence.Goal() - goal_msg.objective_name = objective_name + goal_msg.objective_name = "Move to Joint State" behavior_parameter = BehaviorParameter() behavior_parameter.behavior_namespaces.append("move_to_joint_state") @@ -38,14 +41,14 @@ def send_goal(self, objective_name, waypoint_name="Behind"): def main(args=None): if len(sys.argv) < 2: print( - "usage: ros2 run moveit_studio_behavior call_do_objective.py 'Objective Name'" + "usage: ros2 run moveit_studio_behavior call_do_objective.py 'Waypoint Name'" ) else: rclpy.init(args=args) client = DoObjectiveSequenceClient() - future = client.send_goal(sys.argv[1], sys.argv[2]) + future = client.send_goal(sys.argv[1]) rclpy.spin_until_future_complete(client, future) From 1f52383f0cb916f9ab62b95e6d69e36d8389b9ef Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Thu, 11 May 2023 16:46:28 -0600 Subject: [PATCH 03/18] Updated copyright --- .../scripts/call_do_objective.py | 31 ++++++++++++++++--- .../scripts/call_do_objective_waypoint.py | 31 ++++++++++++++++--- 2 files changed, 54 insertions(+), 8 deletions(-) diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective.py b/src/moveit_studio_agent_utils/scripts/call_do_objective.py index 4bd3500f..96a29a8a 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective.py @@ -1,10 +1,33 @@ #!/usr/bin/env python3 -# Copyright 2022 PickNik Inc. +# Copyright (c) 2023 PickNik Inc. # All rights reserved. -# -# Unauthorized copying of this code base via any medium is strictly prohibited. -# Proprietary and confidential. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: + +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. + +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. + +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. + +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + import rclpy from rclpy.action import ActionClient diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py index 70a3481d..26a8755c 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py @@ -1,10 +1,33 @@ #!/usr/bin/env python3 -# Copyright 2022 PickNik Inc. +# Copyright (c) 2023 PickNik Inc. # All rights reserved. -# -# Unauthorized copying of this code base via any medium is strictly prohibited. -# Proprietary and confidential. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: + +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. + +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. + +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. + +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + import rclpy from rclpy.action import ActionClient From 4061458b68052751ddd35e0d8f2e83ad6d0a01b9 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Fri, 12 May 2023 11:26:08 -0600 Subject: [PATCH 04/18] Reduce package dependencies to a more minimal set --- src/moveit_studio_agent_utils/CMakeLists.txt | 5 ----- src/moveit_studio_agent_utils/package.xml | 2 -- 2 files changed, 7 deletions(-) diff --git a/src/moveit_studio_agent_utils/CMakeLists.txt b/src/moveit_studio_agent_utils/CMakeLists.txt index c9498c26..a092303d 100644 --- a/src/moveit_studio_agent_utils/CMakeLists.txt +++ b/src/moveit_studio_agent_utils/CMakeLists.txt @@ -3,16 +3,11 @@ project(moveit_studio_agent_utils) find_package(ament_cmake REQUIRED) find_package(moveit_studio_agent_msgs REQUIRED) -find_package(moveit_studio_behavior_interface REQUIRED) find_package(moveit_studio_behavior_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(std_msgs REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_agent_msgs - moveit_studio_behavior_interface moveit_studio_behavior_msgs - std_msgs ) ############# diff --git a/src/moveit_studio_agent_utils/package.xml b/src/moveit_studio_agent_utils/package.xml index 7b643905..7ecbc325 100644 --- a/src/moveit_studio_agent_utils/package.xml +++ b/src/moveit_studio_agent_utils/package.xml @@ -14,9 +14,7 @@ moveit_common moveit_studio_agent_msgs - moveit_studio_behavior_interface moveit_studio_behavior_msgs - std_msgs ament_lint_auto ament_cmake_gtest From cb75dc99549fd7e896888e6800852f0c317f9c6d Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Fri, 12 May 2023 12:05:07 -0600 Subject: [PATCH 05/18] Added doc strings and updated call_do_objectives --- .../scripts/call_do_objective.py | 14 +++++++++++++- .../scripts/call_do_objective_waypoint.py | 14 +++++++++++++- 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective.py b/src/moveit_studio_agent_utils/scripts/call_do_objective.py index 96a29a8a..c66c681a 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective.py @@ -38,11 +38,23 @@ class DoObjectiveSequenceClient(Node): + """ + ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server + """ def __init__(self): super().__init__("DoObjectiveSequence") self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") def send_goal(self, objective_name): + """ + Sends a DoObjectiveSequence Goal to the Objective Server via the node's Action Client. + + Args: + objective_name: the (string) name of an objective to run. + + Returns: + result: a BT::NodeStatus result. Can be Success or Failure. + """ goal_msg = DoObjectiveSequence.Goal() goal_msg.objective_name = objective_name self._action_client.wait_for_server() @@ -53,7 +65,7 @@ def send_goal(self, objective_name): def main(args=None): if len(sys.argv) < 2: print( - "usage: ros2 run moveit_studio_behavior call_do_objective.py 'Objective Name'" + "usage: ros2 run moveit_studio_agent_utils call_do_objective.py 'Objective Name'" ) else: rclpy.init(args=args) diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py index 26a8755c..0540c784 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py @@ -41,11 +41,23 @@ ) class DoObjectiveSequenceClient(Node): + """ + ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server + """ def __init__(self): super().__init__("DoObjectiveSequence") self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") def send_goal(self, waypoint_name="Behind"): + """ + Sends a DoObjectiveSequence Goal for "Move to Joint State" to the Objective Server via the node's Action Client. + + Args: + waypoint_name: the (string) name of a waypoint to move to. + + Returns: + result: a BT::NodeStatus result. Can be Success or Failure. + """ goal_msg = DoObjectiveSequence.Goal() goal_msg.objective_name = "Move to Joint State" @@ -64,7 +76,7 @@ def send_goal(self, waypoint_name="Behind"): def main(args=None): if len(sys.argv) < 2: print( - "usage: ros2 run moveit_studio_behavior call_do_objective.py 'Waypoint Name'" + "usage: ros2 run moveit_studio_agent_utils call_do_objective_waypoint.py 'Waypoint Name'" ) else: rclpy.init(args=args) From 9fd59419f579c4dbd210d78fae39ffb961f20c82 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Fri, 12 May 2023 12:06:59 -0600 Subject: [PATCH 06/18] Added precommit config, added license to new package, and then precommit changes --- .gitignore | 2 +- .pre-commit-config.yaml | 73 +++++++++++++++++++ src/moveit_studio_agent_utils/LICENSE | 25 +++++++ src/moveit_studio_agent_utils/README.md | 2 +- .../scripts/call_do_objective.py | 5 +- .../scripts/call_do_objective_waypoint.py | 8 +- src/picknik_ur_base_config/LICENSE | 2 +- .../launch/sim/hardware_sim.launch.py | 6 +- src/picknik_ur_site_config/LICENSE | 2 +- .../config/site_config.yaml | 2 +- 10 files changed, 116 insertions(+), 11 deletions(-) create mode 100644 .pre-commit-config.yaml create mode 100644 src/moveit_studio_agent_utils/LICENSE diff --git a/.gitignore b/.gitignore index 29cad580..13456571 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,3 @@ build/ install/ -log/ \ No newline at end of file +log/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..4d7351df --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,73 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-yaml + args: ["--unsafe"] # Fixes errors parsing custom YAML constructors like ur_description's !degrees + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: fix-byte-order-marker + + - repo: https://github.com/psf/black + rev: 22.3.0 + hooks: + - id: black + + - repo: https://github.com/codespell-project/codespell + rev: v2.0.0 + hooks: + - id: codespell + args: ["--write-changes", "-L", "atleast,inout,ether"] # Provide a comma-separated list of misspelled words that codespell should ignore (for example: '-L', 'word1,word2,word3'). + exclude: \.(svg|pyc|stl|dae|lock)$ + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 + hooks: + - id: clang-format + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|m|proto|vert)$ + # -i arg is included by default by the hook + args: ["-fallback-style=none"] + + - repo: https://github.com/adrienverge/yamllint + rev: v1.27.1 + hooks: + - id: yamllint + args: + [ + "--no-warnings", + "--config-data", + "{extends: default, rules: {line-length: disable, braces: {max-spaces-inside: 1}}}", + ] + types: [text] + files: \.(yml|yaml)$ + + - repo: https://github.com/tcort/markdown-link-check + rev: v3.10.3 + hooks: + - id: markdown-link-check + + # NOTE: Broken on arm64. Will need to bump once https://github.com/hadolint/hadolint/issues/840 is fixed. + - repo: https://github.com/hadolint/hadolint + rev: v2.10.0 + hooks: + - id: hadolint-docker diff --git a/src/moveit_studio_agent_utils/LICENSE b/src/moveit_studio_agent_utils/LICENSE new file mode 100644 index 00000000..574ef079 --- /dev/null +++ b/src/moveit_studio_agent_utils/LICENSE @@ -0,0 +1,25 @@ +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/src/moveit_studio_agent_utils/README.md b/src/moveit_studio_agent_utils/README.md index d2adfcc1..5fb061e7 100644 --- a/src/moveit_studio_agent_utils/README.md +++ b/src/moveit_studio_agent_utils/README.md @@ -1,5 +1,5 @@ # moveit_studio_agent_utils -Provides Scripts to interact with MoveIt Studio Agent API programatically. +Provides Scripts to interact with MoveIt Studio Agent API programmatically. Please see the [Interact with the Objective Server Directly](https://docs.picknik.ai/en/stable/how_to/interact_with_the_objective_server_directly/interact_with_the_objective_server_directly.html) tutorial for more information on these scripts and their use. diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective.py b/src/moveit_studio_agent_utils/scripts/call_do_objective.py index c66c681a..66f4800c 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective.py @@ -41,13 +41,14 @@ class DoObjectiveSequenceClient(Node): """ ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server """ + def __init__(self): super().__init__("DoObjectiveSequence") self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") def send_goal(self, objective_name): - """ - Sends a DoObjectiveSequence Goal to the Objective Server via the node's Action Client. + """ + Sends a DoObjectiveSequence Goal to the Objective Server via the node's Action Client. Args: objective_name: the (string) name of an objective to run. diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py index 0540c784..73249c45 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py @@ -36,21 +36,23 @@ from moveit_studio_agent_msgs.action import DoObjectiveSequence from moveit_studio_behavior_msgs.msg import ( - BehaviorParameter, + BehaviorParameter, BehaviorParameterDescription, ) + class DoObjectiveSequenceClient(Node): """ ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server """ + def __init__(self): super().__init__("DoObjectiveSequence") self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") def send_goal(self, waypoint_name="Behind"): - """ - Sends a DoObjectiveSequence Goal for "Move to Joint State" to the Objective Server via the node's Action Client. + """ + Sends a DoObjectiveSequence Goal for "Move to Joint State" to the Objective Server via the node's Action Client. Args: waypoint_name: the (string) name of a waypoint to move to. diff --git a/src/picknik_ur_base_config/LICENSE b/src/picknik_ur_base_config/LICENSE index f95bc612..574ef079 100644 --- a/src/picknik_ur_base_config/LICENSE +++ b/src/picknik_ur_base_config/LICENSE @@ -22,4 +22,4 @@ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file +POSSIBILITY OF SUCH DAMAGE. diff --git a/src/picknik_ur_gazebo_config/launch/sim/hardware_sim.launch.py b/src/picknik_ur_gazebo_config/launch/sim/hardware_sim.launch.py index 576698ca..55c596a6 100644 --- a/src/picknik_ur_gazebo_config/launch/sim/hardware_sim.launch.py +++ b/src/picknik_ur_gazebo_config/launch/sim/hardware_sim.launch.py @@ -36,7 +36,11 @@ from launch.actions import IncludeLaunchDescription, OpaqueFunction from launch_ros.actions import Node -from moveit_studio_utils_py.launch_common import get_launch_file, get_ros_path, xacro_to_urdf +from moveit_studio_utils_py.launch_common import ( + get_launch_file, + get_ros_path, + xacro_to_urdf, +) from moveit_studio_utils_py.system_config import get_config_folder, SystemConfigParser from moveit_studio_utils_py.generate_camera_frames import generate_camera_frames diff --git a/src/picknik_ur_site_config/LICENSE b/src/picknik_ur_site_config/LICENSE index f95bc612..574ef079 100644 --- a/src/picknik_ur_site_config/LICENSE +++ b/src/picknik_ur_site_config/LICENSE @@ -22,4 +22,4 @@ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file +POSSIBILITY OF SUCH DAMAGE. diff --git a/src/picknik_ur_site_config/config/site_config.yaml b/src/picknik_ur_site_config/config/site_config.yaml index 30ac1963..0cd9bf7c 100644 --- a/src/picknik_ur_site_config/config/site_config.yaml +++ b/src/picknik_ur_site_config/config/site_config.yaml @@ -6,4 +6,4 @@ objectives: # The picknik_ur_base_config uses "core" custom_objectives: package_name: "picknik_ur_site_config" - relative_path: "objectives" \ No newline at end of file + relative_path: "objectives" From 340cb141363bbf47e9382b4913c4bafb0def4cfc Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Fri, 12 May 2023 12:56:16 -0600 Subject: [PATCH 07/18] Renamed from moveit_studio_agent_utils to moveit_studio_agent_examples --- .../CMakeLists.txt | 2 +- .../LICENSE | 0 .../README.md | 2 +- .../package.xml | 2 +- .../scripts/__init__.py | 0 .../scripts/call_do_objective.py | 2 +- .../scripts/call_do_objective_waypoint.py | 2 +- 7 files changed, 5 insertions(+), 5 deletions(-) rename src/{moveit_studio_agent_utils => moveit_studio_agent_examples}/CMakeLists.txt (94%) rename src/{moveit_studio_agent_utils => moveit_studio_agent_examples}/LICENSE (100%) rename src/{moveit_studio_agent_utils => moveit_studio_agent_examples}/README.md (91%) rename src/{moveit_studio_agent_utils => moveit_studio_agent_examples}/package.xml (95%) rename src/{moveit_studio_agent_utils => moveit_studio_agent_examples}/scripts/__init__.py (100%) rename src/{moveit_studio_agent_utils => moveit_studio_agent_examples}/scripts/call_do_objective.py (96%) rename src/{moveit_studio_agent_utils => moveit_studio_agent_examples}/scripts/call_do_objective_waypoint.py (97%) diff --git a/src/moveit_studio_agent_utils/CMakeLists.txt b/src/moveit_studio_agent_examples/CMakeLists.txt similarity index 94% rename from src/moveit_studio_agent_utils/CMakeLists.txt rename to src/moveit_studio_agent_examples/CMakeLists.txt index a092303d..2b98af32 100644 --- a/src/moveit_studio_agent_utils/CMakeLists.txt +++ b/src/moveit_studio_agent_examples/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16.3) -project(moveit_studio_agent_utils) +project(moveit_studio_agent_examples) find_package(ament_cmake REQUIRED) find_package(moveit_studio_agent_msgs REQUIRED) diff --git a/src/moveit_studio_agent_utils/LICENSE b/src/moveit_studio_agent_examples/LICENSE similarity index 100% rename from src/moveit_studio_agent_utils/LICENSE rename to src/moveit_studio_agent_examples/LICENSE diff --git a/src/moveit_studio_agent_utils/README.md b/src/moveit_studio_agent_examples/README.md similarity index 91% rename from src/moveit_studio_agent_utils/README.md rename to src/moveit_studio_agent_examples/README.md index 5fb061e7..7ce36d15 100644 --- a/src/moveit_studio_agent_utils/README.md +++ b/src/moveit_studio_agent_examples/README.md @@ -1,4 +1,4 @@ -# moveit_studio_agent_utils +# moveit_studio_agent_examples Provides Scripts to interact with MoveIt Studio Agent API programmatically. diff --git a/src/moveit_studio_agent_utils/package.xml b/src/moveit_studio_agent_examples/package.xml similarity index 95% rename from src/moveit_studio_agent_utils/package.xml rename to src/moveit_studio_agent_examples/package.xml index 7ecbc325..e10d651c 100644 --- a/src/moveit_studio_agent_utils/package.xml +++ b/src/moveit_studio_agent_examples/package.xml @@ -1,6 +1,6 @@ - moveit_studio_agent_utils + moveit_studio_agent_examples 2.1.0 Package containing scripts for interacting with MoveIt Studio Agent diff --git a/src/moveit_studio_agent_utils/scripts/__init__.py b/src/moveit_studio_agent_examples/scripts/__init__.py similarity index 100% rename from src/moveit_studio_agent_utils/scripts/__init__.py rename to src/moveit_studio_agent_examples/scripts/__init__.py diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py similarity index 96% rename from src/moveit_studio_agent_utils/scripts/call_do_objective.py rename to src/moveit_studio_agent_examples/scripts/call_do_objective.py index 66f4800c..8cc5b081 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -66,7 +66,7 @@ def send_goal(self, objective_name): def main(args=None): if len(sys.argv) < 2: print( - "usage: ros2 run moveit_studio_agent_utils call_do_objective.py 'Objective Name'" + "usage: ros2 run moveit_studio_agent_examples call_do_objective.py 'Objective Name'" ) else: rclpy.init(args=args) diff --git a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py similarity index 97% rename from src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py rename to src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index 73249c45..234a8351 100755 --- a/src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -78,7 +78,7 @@ def send_goal(self, waypoint_name="Behind"): def main(args=None): if len(sys.argv) < 2: print( - "usage: ros2 run moveit_studio_agent_utils call_do_objective_waypoint.py 'Waypoint Name'" + "usage: ros2 run moveit_studio_agent_examples call_do_objective_waypoint.py 'Waypoint Name'" ) else: rclpy.init(args=args) From 144ee9855fa7eb44f567fa8c68160990941e8745 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Fri, 12 May 2023 13:03:19 -0600 Subject: [PATCH 08/18] Some review suggestions. mostly consistency and removing more uneeded packages --- src/moveit_studio_agent_examples/CMakeLists.txt | 2 +- src/moveit_studio_agent_examples/package.xml | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/moveit_studio_agent_examples/CMakeLists.txt b/src/moveit_studio_agent_examples/CMakeLists.txt index 2b98af32..9ac870dc 100644 --- a/src/moveit_studio_agent_examples/CMakeLists.txt +++ b/src/moveit_studio_agent_examples/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(ament_cmake REQUIRED) find_package(moveit_studio_agent_msgs REQUIRED) find_package(moveit_studio_behavior_msgs REQUIRED) +# These are packages needed to link against the necessary MoveIt Studio ROS interface definitions. set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_agent_msgs moveit_studio_behavior_msgs @@ -21,7 +22,6 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/src/moveit_studio_agent_examples/package.xml b/src/moveit_studio_agent_examples/package.xml index e10d651c..ad6ca941 100644 --- a/src/moveit_studio_agent_examples/package.xml +++ b/src/moveit_studio_agent_examples/package.xml @@ -1,18 +1,16 @@ moveit_studio_agent_examples - 2.1.0 + 2.2.0 Package containing scripts for interacting with MoveIt Studio Agent Chance Cardona Chance Cardona - BSD-3 + BSD-3-Clause ament_cmake - moveit_common - moveit_studio_agent_msgs moveit_studio_behavior_msgs From 1e978914c2c0a0d1b3461c550089dd554b4ded65 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Fri, 12 May 2023 18:29:51 -0600 Subject: [PATCH 09/18] Updated docstrings on all the call do objectives. Added a cancelation example. Added response logging for all the scripts --- .../CMakeLists.txt | 1 + .../scripts/call_do_objective.py | 38 +++-- .../scripts/call_do_objective_then_cancel.py | 131 ++++++++++++++++++ .../scripts/call_do_objective_waypoint.py | 34 ++++- 4 files changed, 191 insertions(+), 13 deletions(-) create mode 100755 src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py diff --git a/src/moveit_studio_agent_examples/CMakeLists.txt b/src/moveit_studio_agent_examples/CMakeLists.txt index 9ac870dc..98144b28 100644 --- a/src/moveit_studio_agent_examples/CMakeLists.txt +++ b/src/moveit_studio_agent_examples/CMakeLists.txt @@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS install(PROGRAMS scripts/call_do_objective.py scripts/call_do_objective_waypoint.py + scripts/call_do_objective_then_cancel.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index 8cc5b081..962ec8a1 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -36,7 +36,6 @@ from moveit_studio_agent_msgs.action import DoObjectiveSequence - class DoObjectiveSequenceClient(Node): """ ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server @@ -54,14 +53,36 @@ def send_goal(self, objective_name): objective_name: the (string) name of an objective to run. Returns: - result: a BT::NodeStatus result. Can be Success or Failure. + goal_future: a rclpy.task.Future to a rclpy.action.client.ClientGoalHandle. """ goal_msg = DoObjectiveSequence.Goal() goal_msg.objective_name = objective_name self._action_client.wait_for_server() - result = self._action_client.send_goal_async(goal_msg) - return result - + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + return self._send_goal_future + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info("Goal rejected.") + return + + self._goal_handle = goal_handle + self.get_logger().info("Goal accepted...") + + get_result_future = goal_handle.get_result_async() + get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + if result.error_code.val == 1: + self.get_logger().info(f"Objective succeeded!") + elif hasattr(result.error_code, 'error_message'): + self.get_logger().info(f"Objective failed: {result.error_code.error_message}") + else: + self.get_logger().info(f"Objective failed. MoveItErrorCode Value: {result.error_code.val}") + rclpy.shutdown() def main(args=None): if len(sys.argv) < 2: @@ -72,10 +93,11 @@ def main(args=None): rclpy.init(args=args) client = DoObjectiveSequenceClient() + + objective_name = sys.argv[1] + client.send_goal(objective_name) - future = client.send_goal(sys.argv[1]) - - rclpy.spin_until_future_complete(client, future) + rclpy.spin(client) if __name__ == "__main__": diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py new file mode 100755 index 00000000..3220f245 --- /dev/null +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023 PickNik Inc. +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: + +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. + +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. + +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. + +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +import sys + +from moveit_studio_agent_msgs.action import DoObjectiveSequence + +class DoObjectiveSequenceClient(Node): + """ + ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server + """ + + def __init__(self): + super().__init__("DoObjectiveSequence") + self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") + + def send_goal(self, objective_name): + """ + Sends a DoObjectiveSequence Goal to the Objective Server via the node's Action Client. + + Args: + objective_name: the (string) name of an objective to run. + + Returns: + goal_future: a rclpy.task.Future to a rclpy.action.client.ClientGoalHandle. + """ + goal_msg = DoObjectiveSequence.Goal() + goal_msg.objective_name = objective_name + self._action_client.wait_for_server() + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + return self._send_goal_future + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info("Goal rejected.") + return + + self._goal_handle = goal_handle + self.get_logger().info("Goal accepted...") + + get_result_future = goal_handle.get_result_async() + get_result_future.add_done_callback(self.get_result_callback) + # Cancel after 2 seconds + self._timer = self.create_timer(2.0, self.cancel_goal) + + def get_result_callback(self, future): + result = future.result().result + if result.error_code.val == 1: + self.get_logger().info(f"Objective succeeded!") + elif hasattr(result.error_code, 'error_message'): + self.get_logger().info(f"Objective failed: {result.error_code.error_message}") + else: + self.get_logger().info(f"Objective failed. MoveItErrorCode Value: {result.error_code.val}") + + rclpy.shutdown() + + def cancel_goal(self): + """ + Cancels an Objective Server's DoObjectiveSequence Goal via the node's Action Client. + + Returns: + future: a rclpy.task.Future that completes when the goal is canceled. + """ + self.get_logger().info(f"Attempting to cancel goal.") + future = self._goal_handle.cancel_goal_async() + future.add_done_callback(self.cancel_goal_callback) + # Cancel the timer that this was a part of. + self._timer.cancel() + return future + + def cancel_goal_callback(self, future): + cancel_response = future.result() + if cancel_response.goals_canceling: + self.get_logger().info("Goal successfully canceled.") + else: + self.get_logger().info("Goal failed to cancel.") + + rclpy.shutdown() + + +def main(args=None): + if len(sys.argv) < 2: + print( + "usage: ros2 run moveit_studio_agent_examples call_do_objective.py 'Objective Name'" + ) + else: + rclpy.init(args=args) + + client = DoObjectiveSequenceClient() + + objective_name = sys.argv[1] + client.send_goal(objective_name) + + rclpy.spin(client) + + +if __name__ == "__main__": + main() diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index 234a8351..57e296f4 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -58,7 +58,7 @@ def send_goal(self, waypoint_name="Behind"): waypoint_name: the (string) name of a waypoint to move to. Returns: - result: a BT::NodeStatus result. Can be Success or Failure. + goal_future: a rclpy.task.Future to a rclpy.action.client.ClientGoalHandle. """ goal_msg = DoObjectiveSequence.Goal() goal_msg.objective_name = "Move to Joint State" @@ -71,8 +71,31 @@ def send_goal(self, waypoint_name="Behind"): goal_msg.parameter_overrides = [behavior_parameter] self._action_client.wait_for_server() - result = self._action_client.send_goal_async(goal_msg) - return result + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + return self._send_goal_future + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info("Goal rejected.") + return + + self.get_logger().info("Goal accepted...") + + get_result_future = goal_handle.get_result_async() + get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + if result.error_code.val == 1: + self.get_logger().info(f"Objective succeeded!") + elif hasattr(result.error_code, 'error_message'): + self.get_logger().info(f"Objective failed: {result.error_code.error_message}") + else: + self.get_logger().info(f"Objective failed. MoveItErrorCode Value: {result.error_code.val}") + rclpy.shutdown() + def main(args=None): @@ -85,9 +108,10 @@ def main(args=None): client = DoObjectiveSequenceClient() - future = client.send_goal(sys.argv[1]) + waypoint_name = sys.argv[1] + client.send_goal(waypoint_name) - rclpy.spin_until_future_complete(client, future) + rclpy.spin(client) if __name__ == "__main__": From f1f2dce889e7cac2a1a8c94b2a050115da84df07 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Fri, 12 May 2023 18:34:48 -0600 Subject: [PATCH 10/18] precommit and formatting --- .../scripts/call_do_objective.py | 17 +++++++--- .../scripts/call_do_objective_then_cancel.py | 33 +++++++++++-------- .../scripts/call_do_objective_waypoint.py | 12 ++++--- 3 files changed, 39 insertions(+), 23 deletions(-) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index 962ec8a1..a1734573 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -36,6 +36,7 @@ from moveit_studio_agent_msgs.action import DoObjectiveSequence + class DoObjectiveSequenceClient(Node): """ ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server @@ -67,7 +68,7 @@ def goal_response_callback(self, future): if not goal_handle.accepted: self.get_logger().info("Goal rejected.") return - + self._goal_handle = goal_handle self.get_logger().info("Goal accepted...") @@ -78,12 +79,18 @@ def get_result_callback(self, future): result = future.result().result if result.error_code.val == 1: self.get_logger().info(f"Objective succeeded!") - elif hasattr(result.error_code, 'error_message'): - self.get_logger().info(f"Objective failed: {result.error_code.error_message}") + elif hasattr(result.error_code, "error_message"): + self.get_logger().info( + f"Objective failed: {result.error_code.error_message}" + ) else: - self.get_logger().info(f"Objective failed. MoveItErrorCode Value: {result.error_code.val}") + self.get_logger().info( + f"Objective failed. MoveItErrorCode Value: {result.error_code.val}" + ) + rclpy.shutdown() + def main(args=None): if len(sys.argv) < 2: print( @@ -93,7 +100,7 @@ def main(args=None): rclpy.init(args=args) client = DoObjectiveSequenceClient() - + objective_name = sys.argv[1] client.send_goal(objective_name) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py index 3220f245..b595d2f0 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py @@ -36,6 +36,7 @@ from moveit_studio_agent_msgs.action import DoObjectiveSequence + class DoObjectiveSequenceClient(Node): """ ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server @@ -67,7 +68,7 @@ def goal_response_callback(self, future): if not goal_handle.accepted: self.get_logger().info("Goal rejected.") return - + self._goal_handle = goal_handle self.get_logger().info("Goal accepted...") @@ -80,22 +81,26 @@ def get_result_callback(self, future): result = future.result().result if result.error_code.val == 1: self.get_logger().info(f"Objective succeeded!") - elif hasattr(result.error_code, 'error_message'): - self.get_logger().info(f"Objective failed: {result.error_code.error_message}") + elif hasattr(result.error_code, "error_message"): + self.get_logger().info( + f"Objective failed: {result.error_code.error_message}" + ) else: - self.get_logger().info(f"Objective failed. MoveItErrorCode Value: {result.error_code.val}") + self.get_logger().info( + f"Objective failed. MoveItErrorCode Value: {result.error_code.val}" + ) rclpy.shutdown() - def cancel_goal(self): - """ - Cancels an Objective Server's DoObjectiveSequence Goal via the node's Action Client. + def cancel_goal(self): + """ + Cancels an Objective Server's DoObjectiveSequence Goal via the node's Action Client. Returns: future: a rclpy.task.Future that completes when the goal is canceled. - """ - self.get_logger().info(f"Attempting to cancel goal.") - future = self._goal_handle.cancel_goal_async() + """ + self.get_logger().info(f"Attempting to cancel goal.") + future = self._goal_handle.cancel_goal_async() future.add_done_callback(self.cancel_goal_callback) # Cancel the timer that this was a part of. self._timer.cancel() @@ -103,9 +108,9 @@ def cancel_goal(self): def cancel_goal_callback(self, future): cancel_response = future.result() - if cancel_response.goals_canceling: - self.get_logger().info("Goal successfully canceled.") - else: + if cancel_response.goals_canceling: + self.get_logger().info("Goal successfully canceled.") + else: self.get_logger().info("Goal failed to cancel.") rclpy.shutdown() @@ -120,7 +125,7 @@ def main(args=None): rclpy.init(args=args) client = DoObjectiveSequenceClient() - + objective_name = sys.argv[1] client.send_goal(objective_name) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index 57e296f4..429afb5f 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -90,12 +90,16 @@ def get_result_callback(self, future): result = future.result().result if result.error_code.val == 1: self.get_logger().info(f"Objective succeeded!") - elif hasattr(result.error_code, 'error_message'): - self.get_logger().info(f"Objective failed: {result.error_code.error_message}") + elif hasattr(result.error_code, "error_message"): + self.get_logger().info( + f"Objective failed: {result.error_code.error_message}" + ) else: - self.get_logger().info(f"Objective failed. MoveItErrorCode Value: {result.error_code.val}") - rclpy.shutdown() + self.get_logger().info( + f"Objective failed. MoveItErrorCode Value: {result.error_code.val}" + ) + rclpy.shutdown() def main(args=None): From 33b5633e7280d62035ebfe54219994fa45597463 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Mon, 15 May 2023 13:12:46 -0600 Subject: [PATCH 11/18] Updated to use MoveItErrorCodes. Added argparse and cancellation option --- .../scripts/call_do_objective.py | 3 +- .../scripts/call_do_objective_then_cancel.py | 36 +++++++++++-------- .../scripts/call_do_objective_waypoint.py | 3 +- 3 files changed, 26 insertions(+), 16 deletions(-) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index a1734573..9b137d72 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -34,6 +34,7 @@ from rclpy.node import Node import sys +from moveit_msgs.msg import MoveItErrorCodes from moveit_studio_agent_msgs.action import DoObjectiveSequence @@ -77,7 +78,7 @@ def goal_response_callback(self, future): def get_result_callback(self, future): result = future.result().result - if result.error_code.val == 1: + if result.error_code.val == MoveItErrorCodes.SUCCESS: self.get_logger().info(f"Objective succeeded!") elif hasattr(result.error_code, "error_message"): self.get_logger().info( diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py index b595d2f0..d6a14dc9 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py @@ -28,12 +28,13 @@ # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - +import argparse import rclpy from rclpy.action import ActionClient from rclpy.node import Node import sys +from moveit_msgs.msg import MoveItErrorCodes from moveit_studio_agent_msgs.action import DoObjectiveSequence @@ -46,7 +47,7 @@ def __init__(self): super().__init__("DoObjectiveSequence") self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") - def send_goal(self, objective_name): + def send_goal(self, objective_name, cancel): """ Sends a DoObjectiveSequence Goal to the Objective Server via the node's Action Client. @@ -58,6 +59,7 @@ def send_goal(self, objective_name): """ goal_msg = DoObjectiveSequence.Goal() goal_msg.objective_name = objective_name + self.cancel = cancel self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async(goal_msg) self._send_goal_future.add_done_callback(self.goal_response_callback) @@ -67,6 +69,8 @@ def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info("Goal rejected.") + + rclpy.shutdown() return self._goal_handle = goal_handle @@ -75,11 +79,12 @@ def goal_response_callback(self, future): get_result_future = goal_handle.get_result_async() get_result_future.add_done_callback(self.get_result_callback) # Cancel after 2 seconds - self._timer = self.create_timer(2.0, self.cancel_goal) + if self.cancel: + self._timer = self.create_timer(2.0, self.cancel_goal) def get_result_callback(self, future): result = future.result().result - if result.error_code.val == 1: + if result.error_code.val == MoveItErrorCodes.SUCCESS: self.get_logger().info(f"Objective succeeded!") elif hasattr(result.error_code, "error_message"): self.get_logger().info( @@ -117,19 +122,22 @@ def cancel_goal_callback(self, future): def main(args=None): - if len(sys.argv) < 2: - print( - "usage: ros2 run moveit_studio_agent_examples call_do_objective.py 'Objective Name'" - ) - else: - rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("objective_name", type=str, help="Name of Objective to run.") + parser.add_argument( + "--cancel", + action="store_true", + help="Optional boolean for if the objective should be automatically cancelled after a set amount of time.", + ) + args = parser.parse_args() + + rclpy.init() - client = DoObjectiveSequenceClient() + client = DoObjectiveSequenceClient() - objective_name = sys.argv[1] - client.send_goal(objective_name) + future = client.send_goal(args.objective_name, args.cancel) - rclpy.spin(client) + rclpy.spin(client) if __name__ == "__main__": diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index 429afb5f..f595c842 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -34,6 +34,7 @@ from rclpy.node import Node import sys +from moveit_msgs.msg import MoveItErrorCodes from moveit_studio_agent_msgs.action import DoObjectiveSequence from moveit_studio_behavior_msgs.msg import ( BehaviorParameter, @@ -88,7 +89,7 @@ def goal_response_callback(self, future): def get_result_callback(self, future): result = future.result().result - if result.error_code.val == 1: + if result.error_code.val == MoveItErrorCodes.SUCCESS: self.get_logger().info(f"Objective succeeded!") elif hasattr(result.error_code, "error_message"): self.get_logger().info( From 44733b02a7332289fdfc46fcb60bcf489b2b68e8 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Mon, 15 May 2023 13:37:15 -0600 Subject: [PATCH 12/18] Incorporated cancel option with argparse into both scripts, removed explicit cancel script --- .../CMakeLists.txt | 1 - .../scripts/call_do_objective.py | 56 +++++-- .../scripts/call_do_objective_then_cancel.py | 144 ------------------ .../scripts/call_do_objective_waypoint.py | 59 +++++-- 4 files changed, 90 insertions(+), 170 deletions(-) delete mode 100755 src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py diff --git a/src/moveit_studio_agent_examples/CMakeLists.txt b/src/moveit_studio_agent_examples/CMakeLists.txt index 98144b28..9ac870dc 100644 --- a/src/moveit_studio_agent_examples/CMakeLists.txt +++ b/src/moveit_studio_agent_examples/CMakeLists.txt @@ -19,7 +19,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS install(PROGRAMS scripts/call_do_objective.py scripts/call_do_objective_waypoint.py - scripts/call_do_objective_then_cancel.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index 9b137d72..d6a14dc9 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -28,7 +28,7 @@ # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - +import argparse import rclpy from rclpy.action import ActionClient from rclpy.node import Node @@ -47,7 +47,7 @@ def __init__(self): super().__init__("DoObjectiveSequence") self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") - def send_goal(self, objective_name): + def send_goal(self, objective_name, cancel): """ Sends a DoObjectiveSequence Goal to the Objective Server via the node's Action Client. @@ -59,6 +59,7 @@ def send_goal(self, objective_name): """ goal_msg = DoObjectiveSequence.Goal() goal_msg.objective_name = objective_name + self.cancel = cancel self._action_client.wait_for_server() self._send_goal_future = self._action_client.send_goal_async(goal_msg) self._send_goal_future.add_done_callback(self.goal_response_callback) @@ -68,6 +69,8 @@ def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info("Goal rejected.") + + rclpy.shutdown() return self._goal_handle = goal_handle @@ -75,6 +78,9 @@ def goal_response_callback(self, future): get_result_future = goal_handle.get_result_async() get_result_future.add_done_callback(self.get_result_callback) + # Cancel after 2 seconds + if self.cancel: + self._timer = self.create_timer(2.0, self.cancel_goal) def get_result_callback(self, future): result = future.result().result @@ -91,21 +97,47 @@ def get_result_callback(self, future): rclpy.shutdown() + def cancel_goal(self): + """ + Cancels an Objective Server's DoObjectiveSequence Goal via the node's Action Client. + + Returns: + future: a rclpy.task.Future that completes when the goal is canceled. + """ + self.get_logger().info(f"Attempting to cancel goal.") + future = self._goal_handle.cancel_goal_async() + future.add_done_callback(self.cancel_goal_callback) + # Cancel the timer that this was a part of. + self._timer.cancel() + return future + + def cancel_goal_callback(self, future): + cancel_response = future.result() + if cancel_response.goals_canceling: + self.get_logger().info("Goal successfully canceled.") + else: + self.get_logger().info("Goal failed to cancel.") + + rclpy.shutdown() + def main(args=None): - if len(sys.argv) < 2: - print( - "usage: ros2 run moveit_studio_agent_examples call_do_objective.py 'Objective Name'" - ) - else: - rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("objective_name", type=str, help="Name of Objective to run.") + parser.add_argument( + "--cancel", + action="store_true", + help="Optional boolean for if the objective should be automatically cancelled after a set amount of time.", + ) + args = parser.parse_args() + + rclpy.init() - client = DoObjectiveSequenceClient() + client = DoObjectiveSequenceClient() - objective_name = sys.argv[1] - client.send_goal(objective_name) + future = client.send_goal(args.objective_name, args.cancel) - rclpy.spin(client) + rclpy.spin(client) if __name__ == "__main__": diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py deleted file mode 100755 index d6a14dc9..00000000 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_then_cancel.py +++ /dev/null @@ -1,144 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright (c) 2023 PickNik Inc. -# All rights reserved. - -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: - -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. - -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. - -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. - -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -import argparse -import rclpy -from rclpy.action import ActionClient -from rclpy.node import Node -import sys - -from moveit_msgs.msg import MoveItErrorCodes -from moveit_studio_agent_msgs.action import DoObjectiveSequence - - -class DoObjectiveSequenceClient(Node): - """ - ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server - """ - - def __init__(self): - super().__init__("DoObjectiveSequence") - self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") - - def send_goal(self, objective_name, cancel): - """ - Sends a DoObjectiveSequence Goal to the Objective Server via the node's Action Client. - - Args: - objective_name: the (string) name of an objective to run. - - Returns: - goal_future: a rclpy.task.Future to a rclpy.action.client.ClientGoalHandle. - """ - goal_msg = DoObjectiveSequence.Goal() - goal_msg.objective_name = objective_name - self.cancel = cancel - self._action_client.wait_for_server() - self._send_goal_future = self._action_client.send_goal_async(goal_msg) - self._send_goal_future.add_done_callback(self.goal_response_callback) - return self._send_goal_future - - def goal_response_callback(self, future): - goal_handle = future.result() - if not goal_handle.accepted: - self.get_logger().info("Goal rejected.") - - rclpy.shutdown() - return - - self._goal_handle = goal_handle - self.get_logger().info("Goal accepted...") - - get_result_future = goal_handle.get_result_async() - get_result_future.add_done_callback(self.get_result_callback) - # Cancel after 2 seconds - if self.cancel: - self._timer = self.create_timer(2.0, self.cancel_goal) - - def get_result_callback(self, future): - result = future.result().result - if result.error_code.val == MoveItErrorCodes.SUCCESS: - self.get_logger().info(f"Objective succeeded!") - elif hasattr(result.error_code, "error_message"): - self.get_logger().info( - f"Objective failed: {result.error_code.error_message}" - ) - else: - self.get_logger().info( - f"Objective failed. MoveItErrorCode Value: {result.error_code.val}" - ) - - rclpy.shutdown() - - def cancel_goal(self): - """ - Cancels an Objective Server's DoObjectiveSequence Goal via the node's Action Client. - - Returns: - future: a rclpy.task.Future that completes when the goal is canceled. - """ - self.get_logger().info(f"Attempting to cancel goal.") - future = self._goal_handle.cancel_goal_async() - future.add_done_callback(self.cancel_goal_callback) - # Cancel the timer that this was a part of. - self._timer.cancel() - return future - - def cancel_goal_callback(self, future): - cancel_response = future.result() - if cancel_response.goals_canceling: - self.get_logger().info("Goal successfully canceled.") - else: - self.get_logger().info("Goal failed to cancel.") - - rclpy.shutdown() - - -def main(args=None): - parser = argparse.ArgumentParser() - parser.add_argument("objective_name", type=str, help="Name of Objective to run.") - parser.add_argument( - "--cancel", - action="store_true", - help="Optional boolean for if the objective should be automatically cancelled after a set amount of time.", - ) - args = parser.parse_args() - - rclpy.init() - - client = DoObjectiveSequenceClient() - - future = client.send_goal(args.objective_name, args.cancel) - - rclpy.spin(client) - - -if __name__ == "__main__": - main() diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index f595c842..02b43a23 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -28,7 +28,7 @@ # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - +import argparse import rclpy from rclpy.action import ActionClient from rclpy.node import Node @@ -51,9 +51,9 @@ def __init__(self): super().__init__("DoObjectiveSequence") self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") - def send_goal(self, waypoint_name="Behind"): + def send_goal(self, waypoint_name, cancel): """ - Sends a DoObjectiveSequence Goal for "Move to Joint State" to the Objective Server via the node's Action Client. + Sends a DoObjectiveSequence Goal to the Objective Server via the node's Action Client. Args: waypoint_name: the (string) name of a waypoint to move to. @@ -63,6 +63,7 @@ def send_goal(self, waypoint_name="Behind"): """ goal_msg = DoObjectiveSequence.Goal() goal_msg.objective_name = "Move to Joint State" + self.cancel = cancel behavior_parameter = BehaviorParameter() behavior_parameter.behavior_namespaces.append("move_to_joint_state") @@ -80,12 +81,18 @@ def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: self.get_logger().info("Goal rejected.") + + rclpy.shutdown() return + self._goal_handle = goal_handle self.get_logger().info("Goal accepted...") get_result_future = goal_handle.get_result_async() get_result_future.add_done_callback(self.get_result_callback) + # Cancel after 2 seconds + if self.cancel: + self._timer = self.create_timer(2.0, self.cancel_goal) def get_result_callback(self, future): result = future.result().result @@ -102,21 +109,47 @@ def get_result_callback(self, future): rclpy.shutdown() + def cancel_goal(self): + """ + Cancels an Objective Server's DoObjectiveSequence Goal via the node's Action Client. + + Returns: + future: a rclpy.task.Future that completes when the goal is canceled. + """ + self.get_logger().info(f"Attempting to cancel goal.") + future = self._goal_handle.cancel_goal_async() + future.add_done_callback(self.cancel_goal_callback) + # Cancel the timer that this was a part of. + self._timer.cancel() + return future + + def cancel_goal_callback(self, future): + cancel_response = future.result() + if cancel_response.goals_canceling: + self.get_logger().info("Goal successfully canceled.") + else: + self.get_logger().info("Goal failed to cancel.") + + rclpy.shutdown() + def main(args=None): - if len(sys.argv) < 2: - print( - "usage: ros2 run moveit_studio_agent_examples call_do_objective_waypoint.py 'Waypoint Name'" - ) - else: - rclpy.init(args=args) + parser = argparse.ArgumentParser() + parser.add_argument("waypoint_name", type=str, help="Name of waypoint to move to.") + parser.add_argument( + "--cancel", + action="store_true", + help="Optional boolean for if the objective should be automatically cancelled after a set amount of time.", + ) + args = parser.parse_args() + + rclpy.init() - client = DoObjectiveSequenceClient() + client = DoObjectiveSequenceClient() - waypoint_name = sys.argv[1] - client.send_goal(waypoint_name) + future = client.send_goal(args.waypoint_name, args.cancel) - rclpy.spin(client) + rclpy.spin(client) if __name__ == "__main__": From ae2df5bae0de7041499bd63cb600fefceed24ef7 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Mon, 15 May 2023 13:38:37 -0600 Subject: [PATCH 13/18] update docstring for waypoint script --- .../scripts/call_do_objective_waypoint.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index 02b43a23..d35206ee 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -44,7 +44,8 @@ class DoObjectiveSequenceClient(Node): """ - ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server + ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server. + Configured for the "Move to Joint State" waypoint, and adds a parameter override for waypoint names. """ def __init__(self): From ba39f32842d24364577df32db09d2f6fa33d21f1 Mon Sep 17 00:00:00 2001 From: Chance Cardona <41308677+chancecardona@users.noreply.github.com> Date: Tue, 16 May 2023 14:31:49 -0600 Subject: [PATCH 14/18] Apply suggestions from code review Co-authored-by: Erik Holum --- src/moveit_studio_agent_examples/package.xml | 2 +- src/moveit_studio_agent_examples/scripts/call_do_objective.py | 2 +- .../scripts/call_do_objective_waypoint.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/moveit_studio_agent_examples/package.xml b/src/moveit_studio_agent_examples/package.xml index ad6ca941..6b24fee6 100644 --- a/src/moveit_studio_agent_examples/package.xml +++ b/src/moveit_studio_agent_examples/package.xml @@ -1,7 +1,7 @@ moveit_studio_agent_examples - 2.2.0 + 2.1.0 Package containing scripts for interacting with MoveIt Studio Agent Chance Cardona diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index d6a14dc9..6224d3fe 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -40,7 +40,7 @@ class DoObjectiveSequenceClient(Node): """ - ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server + ROS 2 node that acts as an Action Client for MoveIt Studio's Objective Server. """ def __init__(self): diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index d35206ee..bb6d769d 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -44,7 +44,7 @@ class DoObjectiveSequenceClient(Node): """ - ROS 2 node that acts as an Action Client for the MoveIt Studio Agent's Objective Server. + ROS 2 node that acts as an Action Client for MoveIt Studio's Objective Server. Configured for the "Move to Joint State" waypoint, and adds a parameter override for waypoint names. """ From 1ff1ee1c8044ccae5ef704aa916af7266f0b0b7e Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Tue, 16 May 2023 15:04:20 -0600 Subject: [PATCH 15/18] Remove unneeded fstrings and change hardcoded comment --- .../scripts/call_do_objective.py | 7 +++---- .../scripts/call_do_objective_waypoint.py | 7 +++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index 6224d3fe..4c4d88fe 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -32,7 +32,6 @@ import rclpy from rclpy.action import ActionClient from rclpy.node import Node -import sys from moveit_msgs.msg import MoveItErrorCodes from moveit_studio_agent_msgs.action import DoObjectiveSequence @@ -78,14 +77,14 @@ def goal_response_callback(self, future): get_result_future = goal_handle.get_result_async() get_result_future.add_done_callback(self.get_result_callback) - # Cancel after 2 seconds + # Cancel the goal after a set amount of time (in seconds) if self.cancel: self._timer = self.create_timer(2.0, self.cancel_goal) def get_result_callback(self, future): result = future.result().result if result.error_code.val == MoveItErrorCodes.SUCCESS: - self.get_logger().info(f"Objective succeeded!") + self.get_logger().info("Objective succeeded!") elif hasattr(result.error_code, "error_message"): self.get_logger().info( f"Objective failed: {result.error_code.error_message}" @@ -104,7 +103,7 @@ def cancel_goal(self): Returns: future: a rclpy.task.Future that completes when the goal is canceled. """ - self.get_logger().info(f"Attempting to cancel goal.") + self.get_logger().info("Attempting to cancel goal.") future = self._goal_handle.cancel_goal_async() future.add_done_callback(self.cancel_goal_callback) # Cancel the timer that this was a part of. diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index bb6d769d..14623f12 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -32,7 +32,6 @@ import rclpy from rclpy.action import ActionClient from rclpy.node import Node -import sys from moveit_msgs.msg import MoveItErrorCodes from moveit_studio_agent_msgs.action import DoObjectiveSequence @@ -91,14 +90,14 @@ def goal_response_callback(self, future): get_result_future = goal_handle.get_result_async() get_result_future.add_done_callback(self.get_result_callback) - # Cancel after 2 seconds + # Cancel the goal after a set amount of time (in seconds) if self.cancel: self._timer = self.create_timer(2.0, self.cancel_goal) def get_result_callback(self, future): result = future.result().result if result.error_code.val == MoveItErrorCodes.SUCCESS: - self.get_logger().info(f"Objective succeeded!") + self.get_logger().info("Objective succeeded!") elif hasattr(result.error_code, "error_message"): self.get_logger().info( f"Objective failed: {result.error_code.error_message}" @@ -117,7 +116,7 @@ def cancel_goal(self): Returns: future: a rclpy.task.Future that completes when the goal is canceled. """ - self.get_logger().info(f"Attempting to cancel goal.") + self.get_logger().info("Attempting to cancel goal.") future = self._goal_handle.cancel_goal_async() future.add_done_callback(self.cancel_goal_callback) # Cancel the timer that this was a part of. From d9f7aa34d9a42562876a7645895741ed44a26a2a Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Tue, 16 May 2023 16:39:07 -0600 Subject: [PATCH 16/18] Updated copyright removed unused variable for flake8 --- .../scripts/call_do_objective.py | 15 ++++++++------- .../scripts/call_do_objective_waypoint.py | 15 ++++++++------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index 4c4d88fe..4a474438 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -1,22 +1,23 @@ #!/usr/bin/env python3 -# Copyright (c) 2023 PickNik Inc. +# BSD 3-Clause License +# Copyright (c) 2023, PickNik Inc. # All rights reserved. - +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: - +# # * Redistributions of source code must retain the above copyright notice, this # list of conditions and the following disclaimer. - +# # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. - +# # * Neither the name of the copyright holder nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. - +# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -134,7 +135,7 @@ def main(args=None): client = DoObjectiveSequenceClient() - future = client.send_goal(args.objective_name, args.cancel) + client.send_goal(args.objective_name, args.cancel) rclpy.spin(client) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index 14623f12..a8ec5f20 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -1,22 +1,23 @@ #!/usr/bin/env python3 -# Copyright (c) 2023 PickNik Inc. +# BSD 3-Clause License +# Copyright (c) 2023, PickNik Inc. # All rights reserved. - +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: - +# # * Redistributions of source code must retain the above copyright notice, this # list of conditions and the following disclaimer. - +# # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. - +# # * Neither the name of the copyright holder nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. - +# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -147,7 +148,7 @@ def main(args=None): client = DoObjectiveSequenceClient() - future = client.send_goal(args.waypoint_name, args.cancel) + client.send_goal(args.waypoint_name, args.cancel) rclpy.spin(client) From dee26e8a0d97a8b0c741771843a57a1c163e460e Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Wed, 17 May 2023 10:05:32 -0600 Subject: [PATCH 17/18] Fixed copyrights for real via ament_copyright add-missing --- .../scripts/call_do_objective.py | 38 +++++++++---------- .../scripts/call_do_objective_waypoint.py | 38 +++++++++---------- 2 files changed, 38 insertions(+), 38 deletions(-) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index 4a474438..95218ebc 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -1,33 +1,33 @@ #!/usr/bin/env python3 -# BSD 3-Clause License -# Copyright (c) 2023, PickNik Inc. -# All rights reserved. +# Copyright 2023 Picknik Inc. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. # -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. # -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * Neither the name of the Picknik Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + import argparse import rclpy diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index a8ec5f20..4b1e4a51 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -1,33 +1,33 @@ #!/usr/bin/env python3 -# BSD 3-Clause License -# Copyright (c) 2023, PickNik Inc. -# All rights reserved. +# Copyright 2023 Picknik Inc. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. # -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. # -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * Neither the name of the Picknik Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + import argparse import rclpy From e10b9d8aacd1d26ce1df539c48e666812f3df009 Mon Sep 17 00:00:00 2001 From: Chance Cardona Date: Wed, 17 May 2023 10:09:41 -0600 Subject: [PATCH 18/18] Fix sonarcloud bug --- src/moveit_studio_agent_examples/scripts/call_do_objective.py | 2 +- .../scripts/call_do_objective_waypoint.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py index 95218ebc..2d66db02 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective.py @@ -121,7 +121,7 @@ def cancel_goal_callback(self, future): rclpy.shutdown() -def main(args=None): +def main(): parser = argparse.ArgumentParser() parser.add_argument("objective_name", type=str, help="Name of Objective to run.") parser.add_argument( diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py index 4b1e4a51..3bbb46a5 100755 --- a/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -134,7 +134,7 @@ def cancel_goal_callback(self, future): rclpy.shutdown() -def main(args=None): +def main(): parser = argparse.ArgumentParser() parser.add_argument("waypoint_name", type=str, help="Name of waypoint to move to.") parser.add_argument(