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_examples/CMakeLists.txt b/src/moveit_studio_agent_examples/CMakeLists.txt new file mode 100644 index 00000000..9ac870dc --- /dev/null +++ b/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() diff --git a/src/moveit_studio_agent_examples/LICENSE b/src/moveit_studio_agent_examples/LICENSE new file mode 100644 index 00000000..574ef079 --- /dev/null +++ b/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. diff --git a/src/moveit_studio_agent_examples/README.md b/src/moveit_studio_agent_examples/README.md new file mode 100644 index 00000000..7ce36d15 --- /dev/null +++ b/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. diff --git a/src/moveit_studio_agent_examples/package.xml b/src/moveit_studio_agent_examples/package.xml new file mode 100644 index 00000000..6b24fee6 --- /dev/null +++ b/src/moveit_studio_agent_examples/package.xml @@ -0,0 +1,28 @@ + + + moveit_studio_agent_examples + 2.1.0 + Package containing scripts for interacting with MoveIt Studio Agent + + Chance Cardona + Chance Cardona + + BSD-3-Clause + + ament_cmake + + moveit_studio_agent_msgs + moveit_studio_behavior_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_examples/scripts/__init__.py b/src/moveit_studio_agent_examples/scripts/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/src/moveit_studio_agent_examples/scripts/call_do_objective.py b/src/moveit_studio_agent_examples/scripts/call_do_objective.py new file mode 100755 index 00000000..2d66db02 --- /dev/null +++ b/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() 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 new file mode 100755 index 00000000..3bbb46a5 --- /dev/null +++ b/src/moveit_studio_agent_examples/scripts/call_do_objective_waypoint.py @@ -0,0 +1,157 @@ +#!/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 +from moveit_studio_behavior_msgs.msg import ( + BehaviorParameter, + BehaviorParameterDescription, +) + + +class DoObjectiveSequenceClient(Node): + """ + 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. + """ + + def __init__(self): + super().__init__("DoObjectiveSequence") + self._action_client = ActionClient(self, DoObjectiveSequence, "do_objective") + + def send_goal(self, waypoint_name, cancel): + """ + 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. + + Returns: + goal_future: a rclpy.task.Future to a rclpy.action.client.ClientGoalHandle. + """ + 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") + 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() + 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("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.send_goal(args.waypoint_name, args.cancel) + + rclpy.spin(client) + + +if __name__ == "__main__": + main() 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"