Add Example Agent call_do_objective.py Scripts

.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_utils/CMakeLists.txt

@@ -0,0 +1,31 @@
+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_msgs REQUIRED)
+
+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_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.

src/moveit_studio_agent_utils/README.md

@@ -0,0 +1,5 @@
+# moveit_studio_agent_utils
+
+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_utils/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>moveit_studio_agent_utils</name>
+  <version>2.1.0</version>
+  <description>Package containing scripts for interacting with MoveIt Studio Agent</description>
+
+  <maintainer email="[email protected]">Chance Cardona</maintainer>
+  <author email="[email protected]">Chance Cardona</author>
+
+  <license>BSD-3</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>moveit_common</build_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_utils/scripts/call_do_objective.py

@@ -0,0 +1,82 @@
+#!/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:
+            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()
+        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_agent_utils 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()

src/moveit_studio_agent_utils/scripts/call_do_objective_waypoint.py

@@ -0,0 +1,94 @@
+#!/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
+from moveit_studio_behavior_msgs.msg import (
+    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.
+
+        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"
+
+        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_agent_utils call_do_objective_waypoint.py 'Waypoint 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()

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.
+POSSIBILITY OF SUCH DAMAGE.

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
 

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.
+POSSIBILITY OF SUCH DAMAGE.

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"
+      relative_path: "objectives"