Add Example Agent call_do_objective.py Scripts (#16)

* Added the example call_do_objective scripts

* Updated call_do_objective_waypoint to be consistent

* Updated copyright

* Reduce package dependencies to a more minimal set

* Added doc strings and updated call_do_objectives

* Added precommit config, added license to new package, and then precommit changes

* Renamed from moveit_studio_agent_utils to moveit_studio_agent_examples

* Some review suggestions. mostly consistency and removing more uneeded packages

* Updated docstrings on all the call do objectives. Added a cancelation example. Added response logging for all the scripts

* precommit and formatting

* Updated to use MoveItErrorCodes. Added argparse and cancellation option

* Incorporated cancel option with argparse into both scripts, removed explicit cancel script

* update docstring for waypoint script

* Apply suggestions from code review

Co-authored-by: Erik Holum <erik.holum@picknik.ai>

* Remove unneeded fstrings and change hardcoded comment

* Updated copyright removed unused variable for flake8

* Fixed copyrights for real via ament_copyright add-missing

* Fix sonarcloud bug

---------

Co-authored-by: Erik Holum <erik.holum@picknik.ai>

.gitignore

@@ -1,3 +1,3 @@
 build/
 install/
-log/
+log/

.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

src/moveit_studio_agent_examples/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.16.3)
+project(moveit_studio_agent_examples)
+
+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
+)
+
+#############
+## 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()

src/moveit_studio_agent_examples/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.

src/moveit_studio_agent_examples/README.md

@@ -0,0 +1,5 @@
+# moveit_studio_agent_examples
+
+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.

src/moveit_studio_agent_examples/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>moveit_studio_agent_examples</name>
+  <version>2.1.0</version>
+  <description>Package containing scripts for interacting with MoveIt Studio Agent</description>
+
+  <maintainer email="chance.cardona@picknik.ai">Chance Cardona</maintainer>
+  <author email="chance.cardona@picknik.ai">Chance Cardona</author>
+
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>moveit_studio_agent_msgs</depend>
+  <depend>moveit_studio_behavior_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_clang_tidy</test_depend>
+  <test_depend>ament_cmake_copyright</test_depend>
+  <test_depend>ament_cmake_lint_cmake</test_depend>
+  <test_depend>picknik_ament_copyright</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/moveit_studio_agent_examples/scripts/call_do_objective.py

@@ -0,0 +1,144 @@
+#!/usr/bin/env python3
+
+# 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 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 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.
+
+
+import argparse
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+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 MoveIt Studio'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 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("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("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():
+    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.send_goal(args.objective_name, args.cancel)
+
+    rclpy.spin(client)
+
+
+if __name__ == "__main__":
+    main()