From 478f40d224aeca6e7b754e1a8bb933835f90fa42 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 27 Sep 2024 16:40:12 -0700 Subject: [PATCH 01/26] Change SSH username to nano@nano --- start.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/start.py b/start.py index c4184c79..0b695f21 100644 --- a/start.py +++ b/start.py @@ -271,7 +271,7 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "pm2 log server", ], "camera": [ - "ssh nano -t './start_nano.sh'", + "ssh nano@nano -t './start_nano.sh'", ], "ft": [ "ros2 run forque_sensor_hardware forque_sensor_hardware --ros-args -p host:=ft-sensor-2", From f2115c016292951879486657a21ebbf8461f5b24 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 4 Oct 2024 10:05:02 -0700 Subject: [PATCH 02/26] Skip Lovelace config --- start.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/start.py b/start.py index 0b695f21..bfc9ec36 100644 --- a/start.py +++ b/start.py @@ -283,12 +283,12 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "ros2 launch ada_feeding_perception ada_feeding_perception.launch.py combine_perception_nodes:=true", ], "moveit": [ - "Xvfb :5 -screen 0 800x600x24 &" if not args.dev else "", - "export DISPLAY=:5" if not args.dev else "", + # "Xvfb :5 -screen 0 800x600x24 &" if not args.dev else "", + # "export DISPLAY=:5" if not args.dev else "", f"ros2 launch ada_planning_scene ada_moveit_launch.xml use_rviz:={'true' if args.dev else 'false'}", ], "feeding": [ - "sudo ./src/ada_feeding/configure_lovelace.sh", + # "sudo ./src/ada_feeding/configure_lovelace.sh", ( "ros2 launch ada_feeding ada_feeding_launch.xml " f"use_estop:={'false' if args.dev else 'true'} run_web_bridge:=false policy:={args.policy}" From 0130e83eedc704c0873daca64e8db313dd4fd282 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 15 Dec 2024 20:14:27 -0800 Subject: [PATCH 03/26] Add end_effector_tool configuration --- ada_planning_scene/launch/ada_moveit_launch.xml | 2 ++ start.py | 14 ++++++++++++-- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/ada_planning_scene/launch/ada_moveit_launch.xml b/ada_planning_scene/launch/ada_moveit_launch.xml index fde30ae3..0fb72a00 100644 --- a/ada_planning_scene/launch/ada_moveit_launch.xml +++ b/ada_planning_scene/launch/ada_moveit_launch.xml @@ -5,6 +5,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/start.py b/start.py index bfc9ec36..be04b02a 100644 --- a/start.py +++ b/start.py @@ -71,6 +71,13 @@ "These options are all named learning policies for ada_feeding_action_select." ), ) +parser.add_argument( + "--end_effector_tool", + default="fork", + help=( + "Which end-effector tool to use: 'none', 'fork', 'articulable_fork'" + ) +) async def get_existing_screens(): @@ -208,7 +215,8 @@ async def main(args: argparse.Namespace, pwd: str) -> None: ), ], "moveit": [ - "ros2 launch ada_planning_scene ada_moveit_launch.xml sim:=mock" + "ros2 launch ada_planning_scene ada_moveit_launch.xml sim:=mock " + f"end_effector_tool:={args.end_effector_tool}", ], "browser": [ "cd ./src/feeding_web_interface/feedingwebapp", @@ -285,7 +293,9 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "moveit": [ # "Xvfb :5 -screen 0 800x600x24 &" if not args.dev else "", # "export DISPLAY=:5" if not args.dev else "", - f"ros2 launch ada_planning_scene ada_moveit_launch.xml use_rviz:={'true' if args.dev else 'false'}", + "ros2 launch ada_planning_scene ada_moveit_launch.xml " + f"use_rviz:={'true' if args.dev else 'false'}" + f"end_effector_tool:={args.end_effector_tool}", ], "feeding": [ # "sudo ./src/ada_feeding/configure_lovelace.sh", From 7bd48d4176ebd2312903ca31aff14be242cbb330 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 15 Dec 2024 21:21:37 -0800 Subject: [PATCH 04/26] Use ada move group for feeding --- ada_feeding/ada_feeding/helpers.py | 2 +- .../ada_planning_scene/collision_object_manager.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 1dba6868..3de43aef 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -359,7 +359,7 @@ def get_moveit2_object( joint_names=kinova.joint_names(), base_link_name=kinova.base_link_name(), end_effector_name="forkTip", - group_name="jaco_arm", + group_name="ada", callback_group=callback_group, ) lock = Lock() diff --git a/ada_planning_scene/ada_planning_scene/collision_object_manager.py b/ada_planning_scene/ada_planning_scene/collision_object_manager.py index ce3e34a1..294b0101 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -52,7 +52,7 @@ def __init__(self, node: Node): joint_names=kinova.joint_names(), base_link_name=kinova.base_link_name(), end_effector_name="forkTip", - group_name="jaco_arm", + group_name="ada", callback_group=callback_group, ) From 2b965b1bb99b31dfeb91fcc335d45d00a57db3d2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 16 Dec 2024 11:47:22 -0800 Subject: [PATCH 05/26] Add space in real moveit launch --- start.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/start.py b/start.py index be04b02a..9cc8c31e 100644 --- a/start.py +++ b/start.py @@ -294,7 +294,7 @@ async def main(args: argparse.Namespace, pwd: str) -> None: # "Xvfb :5 -screen 0 800x600x24 &" if not args.dev else "", # "export DISPLAY=:5" if not args.dev else "", "ros2 launch ada_planning_scene ada_moveit_launch.xml " - f"use_rviz:={'true' if args.dev else 'false'}" + f"use_rviz:={'true' if args.dev else 'false'} " f"end_effector_tool:={args.end_effector_tool}", ], "feeding": [ From 6a098d800f0450cf7156cd31c76f3165c5a31e3e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 18 Dec 2024 07:55:54 -0800 Subject: [PATCH 06/26] Restore lovelace configuration --- start.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/start.py b/start.py index 9cc8c31e..012e9011 100644 --- a/start.py +++ b/start.py @@ -279,7 +279,7 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "pm2 log server", ], "camera": [ - "ssh nano@nano -t './start_nano.sh'", + "ssh nano -t './start_nano.sh'", ], "ft": [ "ros2 run forque_sensor_hardware forque_sensor_hardware --ros-args -p host:=ft-sensor-2", @@ -291,14 +291,14 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "ros2 launch ada_feeding_perception ada_feeding_perception.launch.py combine_perception_nodes:=true", ], "moveit": [ - # "Xvfb :5 -screen 0 800x600x24 &" if not args.dev else "", - # "export DISPLAY=:5" if not args.dev else "", + "Xvfb :5 -screen 0 800x600x24 &" if not args.dev else "", + "export DISPLAY=:5" if not args.dev else "", "ros2 launch ada_planning_scene ada_moveit_launch.xml " f"use_rviz:={'true' if args.dev else 'false'} " f"end_effector_tool:={args.end_effector_tool}", ], "feeding": [ - # "sudo ./src/ada_feeding/configure_lovelace.sh", + "sudo ./src/ada_feeding/configure_lovelace.sh", ( "ros2 launch ada_feeding ada_feeding_launch.xml " f"use_estop:={'false' if args.dev else 'true'} run_web_bridge:=false policy:={args.policy}" From f8edaafba3e307aeabeed1ccf4f227de426db04c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 18 Dec 2024 08:19:47 -0800 Subject: [PATCH 07/26] Reformatting with black --- start.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/start.py b/start.py index 1cd475ea..67904aeb 100755 --- a/start.py +++ b/start.py @@ -74,9 +74,7 @@ parser.add_argument( "--end_effector_tool", default="fork", - help=( - "Which end-effector tool to use: 'none', 'fork', 'articulable_fork'" - ) + help=("Which end-effector tool to use: 'none', 'fork', 'articulable_fork'"), ) From 7da9b5fd9873b133eea255c45a1aa27522a37dbe Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:22:36 -0800 Subject: [PATCH 08/26] Additionally exclude AF joints if is set to True --- .../ada_feeding/behaviors/moveit2/moveit2_plan.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py index 200129bf..b985d218 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py @@ -531,8 +531,14 @@ def get_path_len( return total_len, joint_lens j6_i = None + af1_i = None + af2_i = None if exclude_j6 and "j2n6s200_joint_6" in path.joint_names: j6_i = path.joint_names.index("j2n6s200_joint_6") + if exclude_j6 and "af_joint_1" in path.joint_names: + af1_i = path.joint_names.index("af_joint_1") + if exclude_j6 and "af_joint_2" in path.joint_names: + af2_i = path.joint_names.index("af_joint_2") prev_pos = np.array(path.points[0].positions) for point in path.points: @@ -540,9 +546,15 @@ def get_path_len( seg_len = np.abs(curr_pos - prev_pos) if j6_i is not None: j6_len = seg_len[j6_i] + af1_len = seg_len[af1_i] + af2_len = seg_len[af2_i] seg_len[j6_i] = 0.0 + seg_len[af1_i] = 0.0 + seg_len[af2_i] = 0.0 total_len += np.linalg.norm(seg_len) seg_len[j6_i] = j6_len + seg_len[af1_i] = af1_len + seg_len[af2_i] = af2_len else: total_len += np.linalg.norm(seg_len) for index, name in enumerate(path.joint_names): From eeebd3c2f79ce697b6932972af5c107ad9a92662 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:25:16 -0800 Subject: [PATCH 09/26] Define get_tool_joints, a function that returns the list of joints associated with the selected end_effector_tool, and use it when defining the list of joint_names in the MoveIt2 object --- ada_feeding/ada_feeding/helpers.py | 38 ++++++++++++++++++++++++++++-- 1 file changed, 36 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 0c5d6fb2..a54b62fd 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -9,7 +9,7 @@ # Standard imports import logging from threading import Lock -from typing import Any, Optional, Set, Tuple +from typing import Any, Optional, Set, Tuple, List # Third-party imports import numpy as np @@ -331,12 +331,15 @@ def get_moveit2_object( # the same object. moveit2_blackboard_key = "/moveit2" moveit2_lock_blackboard_key = "/moveit2_lock" + end_effector_tool_blackboard_key = "/end_effector_tool" # First, register the MoveIt2 object and its corresponding lock for READ access if not blackboard.is_registered(moveit2_blackboard_key, Access.READ): blackboard.register_key(moveit2_blackboard_key, Access.READ) if not blackboard.is_registered(moveit2_lock_blackboard_key, Access.READ): blackboard.register_key(moveit2_lock_blackboard_key, Access.READ) + if not blackboard.is_registered(end_effector_tool_blackboard_key, Access.READ): + blackboard.register_key(end_effector_tool_blackboard_key, Access.READ) # Second, check if the MoveIt2 object and its corresponding lock exist on the # blackboard. If they do not, register the blackboard for WRITE access to those @@ -357,9 +360,14 @@ def get_moveit2_object( blackboard.register_key(moveit2_lock_blackboard_key, Access.WRITE) # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. callback_group = ReentrantCallbackGroup() + tool_joints = get_tool_joints(blackboard.get(end_effector_tool_blackboard_key)) + joint_names = kinova.joint_names() + tool_joints + node.get_logger().info( + f"Creating MoveIt2 object with: {joint_names}" + ) moveit2 = MoveIt2( node=node, - joint_names=kinova.joint_names(), + joint_names=joint_names, base_link_name=kinova.base_link_name(), end_effector_name="forkTip", group_name="ada", @@ -454,3 +462,29 @@ def import_from_string(import_string: str) -> Any: ) except Exception as exc: raise ImportError(f"Error importing {import_string}") from exc + +def get_tool_joints(end_effector_tool: str) -> List[str]: + """ + Returns a list of joint names associated with the given end-effector tool. + + Args: + end_effector_tool: The name of the end-effector tool. + + Returns: + A list of joint names, or an empty list if no joints are associated + with the tool. + + Raises: + ValueError: If an unsupported tool is provided. + """ + + tool_joints = { + "articulable_fork": ["af_joint_1", "af_joint_2"], + "none": [], + "fork": [], + } + + if end_effector_tool in tool_joints: + return tool_joints[end_effector_tool] + else: + raise ValueError(f"Unknown end_effector_tool: {end_effector_tool}") From bd1af208d87aac30780cdd0dce5dd7ffb7ad01a7 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:25:54 -0800 Subject: [PATCH 10/26] Add af_controller and af_servo_controller to ActivateControllerTree --- ada_feeding/ada_feeding/trees/activate_controller.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/activate_controller.py b/ada_feeding/ada_feeding/trees/activate_controller.py index 6cc588c9..e6518601 100644 --- a/ada_feeding/ada_feeding/trees/activate_controller.py +++ b/ada_feeding/ada_feeding/trees/activate_controller.py @@ -60,6 +60,8 @@ def __init__( "jaco_arm_cartesian_controller", "jaco_arm_controller", "jaco_arm_servo_controller", + "af_controller", + "af_servo_controller", ] self.re_tare = re_tare From 5d6f9c49a98870f19e1481cf877e5c116e08bc90 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:28:38 -0800 Subject: [PATCH 11/26] Remove assertions that assume a static set of 6 joints --- ada_feeding/ada_feeding/trees/move_from_mouth_tree.py | 2 -- .../trees/move_to_configuration_with_ft_thresholds_tree.py | 3 --- .../trees/move_to_configuration_with_wheelchair_wall_tree.py | 1 - 3 files changed, 6 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py index 4ae6c17c..de5529c5 100644 --- a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py @@ -152,8 +152,6 @@ def __init__( self.staging_configuration_position = staging_configuration_position self.staging_configuration_quat_xyzw = staging_configuration_quat_xyzw self.end_configuration = end_configuration - if self.end_configuration is not None: - assert len(self.end_configuration) == 6, "Must provide 6 joint positions" self.staging_configuration_tolerance_position = ( staging_configuration_tolerance_position ) diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py index b1a44a43..eabdfd89 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py @@ -264,9 +264,6 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: assert ( self.joint_positions is not None ), "For action MoveTo, must provide hardcoded joint_positions" - assert ( - len(self.joint_positions) == 6 - ), "For action MoveTo, must provide 6 joint positions" # Adds MoveToVisitor for Feedback return super().send_goal(tree, goal) diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py index 50bd68ba..bae58451 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py @@ -95,7 +95,6 @@ def __init__( # Store the parameters self.goal_configuration = goal_configuration - assert len(self.goal_configuration) == 6, "Must provide 6 joint positions" self.goal_configuration_tolerance = goal_configuration_tolerance self.orientation_constraint_quaternion = orientation_constraint_quaternion self.orientation_constraint_tolerances = orientation_constraint_tolerances From ffaa1ab76082ae545fb2214db528635b00cde499 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:33:13 -0800 Subject: [PATCH 12/26] Adjust MoveAbovePlate configurations to account for extra length from Articulable Fork --- .../config/ada_feeding_action_servers_custom.yaml | 12 ++++++------ .../config/ada_feeding_action_servers_default.yaml | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index fb5636ae..3a66f97a 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -118,12 +118,12 @@ ada_feeding_action_servers: - -2.0289974534007733 - -3.1847696853949206 MoveAbovePlate.tree_kwargs.joint_positions: - - 6.794832881070045 - - 3.031285852819256 - - 4.479355460783922 - - 7.187106922258792 - - -1.9369287787234262 - - -3.5496749526931417 + - -2.4538579336877304 + - 3.07974419938212 + - 1.8320725365979 + - 4.096143890468605 + - -2.003422584820525 + - -3.2123560395465063 MoveFromMouth.tree_kwargs.staging_configuration_position: - -0.6081959669757366 - 0.07835060871598665 diff --git a/ada_feeding/config/ada_feeding_action_servers_default.yaml b/ada_feeding/config/ada_feeding_action_servers_default.yaml index 70fd7daf..20183e7d 100644 --- a/ada_feeding/config/ada_feeding_action_servers_default.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_default.yaml @@ -39,12 +39,12 @@ ada_feeding_action_servers: - max_velocity_scaling_factor tree_kwargs: # optional joint_positions: # required - - -2.3149168248766614 # j2n6s200_joint_1 - - 3.1444595465032634 # j2n6s200_joint_2 - - 1.7332586075115999 # j2n6s200_joint_3 - - -2.3609596843308234 # j2n6s200_joint_4 - - 4.43936623280362 # j2n6s200_joint_5 - - 3.06866544924739 # j2n6s200_joint_6 + - -2.4538579336877304 + - 3.07974419938212 + - 1.8320725365979 + - 4.096143890468605 + - -2.003422584820525 + - -3.2123560395465063 toggle_watchdog_listener: false # optional, default: true f_mag: 4.0 # N max_velocity_scaling_factor: 1.0 # optional in (0.0, 1.0], default: 0.1 From 7debd19fa95a73d7ea76cac23ca7b182bde835fe Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:35:42 -0800 Subject: [PATCH 13/26] Add end_effector_tool argument to ada_feeding_launch.xml and pass it as a parameter to ada_feeding_action_servers --- ada_feeding/launch/ada_feeding_launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ada_feeding/launch/ada_feeding_launch.xml b/ada_feeding/launch/ada_feeding_launch.xml index 7c2bf870..10a7ea5d 100644 --- a/ada_feeding/launch/ada_feeding_launch.xml +++ b/ada_feeding/launch/ada_feeding_launch.xml @@ -9,6 +9,7 @@ + @@ -26,6 +27,7 @@ + From 2aa2b39f4477d58d0b3f153cc3a18066ec1e065c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:38:23 -0800 Subject: [PATCH 14/26] Set end_effector_tool on global blackboard key, and zero-pad joint configurations based on tool_joints associated with end_effector_tool --- ada_feeding/scripts/create_action_servers.py | 42 ++++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index a0a98413..dc094d04 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -20,6 +20,7 @@ from ada_watchdog_listener import ADAWatchdogListener from ament_index_python.packages import get_package_share_directory import py_trees +from py_trees.common import Access from rcl_interfaces.msg import ( Parameter as ParameterMsg, ParameterDescriptor, @@ -33,13 +34,14 @@ from rclpy.action.server import ServerGoalHandle from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.executors import MultiThreadedExecutor +from rclpy.exceptions import ParameterNotDeclaredException from rclpy.node import Node from rclpy.parameter import Parameter import yaml # Local imports from ada_feeding import ActionServerBT -from ada_feeding.helpers import import_from_string, register_logger +from ada_feeding.helpers import import_from_string, register_logger, get_tool_joints from ada_feeding.visitors import DebugVisitor @@ -115,7 +117,7 @@ def __init__(self) -> None: super().__init__("create_action_servers", allow_undeclared_parameters=True) register_logger(self.get_logger()) - def initialize(self) -> None: + def initialize(self, blackboard: py_trees.blackboard.Client) -> None: """ Initialize the node. This is a separate function from above so rclpy can be spinning while this function is called. @@ -134,6 +136,22 @@ def initialize(self) -> None: callback_group=MutuallyExclusiveCallbackGroup(), ) + try: + # Get the end_effector_tool parameter + self.declare_parameter('end_effector_tool') # Declaring the parameter first + self.end_effector_tool = self.get_parameter('end_effector_tool').value + self.get_logger().info(f"End effector tool: {self.end_effector_tool}") + except ParameterNotDeclaredException: + self.get_logger().warn("Parameter 'end_effector_tool' not declared. Using default value.") + self.end_effector_tool = "fork" # Provide a sensible default + + self.tool_joints = get_tool_joints(self.end_effector_tool) + self.get_logger().info(f"Tool joints: {self.tool_joints}") + + # Set the end_effector_tool on the global blackboard using the provided client + blackboard.register_key("/end_effector_tool", Access.WRITE) + blackboard.set("/end_effector_tool", self.end_effector_tool) # Set the global key + # Read the parameters that specify what action servers to create. self.namespace_to_use = CreateActionServers.DEFAULT_PARAMETER_NAMESPACE self.action_server_params = self.read_params() @@ -351,11 +369,19 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam full_name ] = CreateActionServers.get_parameter_value(custom_value) if self.parameters[self.namespace_to_use][full_name] is not None: - tree_kwargs[kw] = self.parameters[self.namespace_to_use][ - full_name - ] + value = self.parameters[self.namespace_to_use][full_name] else: - tree_kwargs[kw] = self.parameters[default_namespace][full_name] + value = self.parameters[default_namespace][full_name] + + if kw.endswith("joint_positions") or kw.endswith("goal_configuration"): + if isinstance(value, list): # Check if the value is a list + zeros_to_append = [0.0] * len(self.tool_joints) + value.extend(zeros_to_append) # Append tool joints + self.get_logger().info(f"Appending tool joints to {full_name}: {self.tool_joints}") + self.get_logger().info(f"Value of {full_name} after appending: {value}") + else: + self.get_logger().warn(f"tree_kwarg {full_name} is not a list, cannot append tool joints") + tree_kwargs[kw] = value action_server_params[server_name] = ActionServerParams( server_name=server_name, @@ -1159,6 +1185,8 @@ def main(args: List = None) -> None: create_action_servers = CreateActionServers() + blackboard = py_trees.blackboard.Client() + # Use a MultiThreadedExecutor to enable processing goals concurrently executor = MultiThreadedExecutor(num_threads=multiprocessing.cpu_count() * 2) @@ -1175,7 +1203,7 @@ def main(args: List = None) -> None: # All exceptions need printing at shutdown try: # Initialize the node - create_action_servers.initialize() + create_action_servers.initialize(blackboard) # Spin in the foreground spin_thread.join() From 9ae91c830d97e081a39ff181472363083c776326 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:45:03 -0800 Subject: [PATCH 15/26] Modify joint_names in collision_object_manager and workspace_walls based on tool_joints defined by end_effector_tool --- .../ada_planning_scene/ada_planning_scene.py | 17 ++++++++++++++++- .../collision_object_manager.py | 7 ++++--- .../ada_planning_scene/workspace_walls.py | 8 +++++--- 3 files changed, 25 insertions(+), 7 deletions(-) diff --git a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py index 64293bf4..094ada6a 100755 --- a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py +++ b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py @@ -15,6 +15,7 @@ from rcl_interfaces.msg import ParameterDescriptor, ParameterType, SetParametersResult from rcl_interfaces.srv import GetParameters import rclpy +from rclpy.exceptions import ParameterNotDeclaredException from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.duration import Duration from rclpy.executors import MultiThreadedExecutor @@ -31,6 +32,7 @@ from ada_planning_scene.update_from_face_detection import UpdateFromFaceDetection from ada_planning_scene.update_from_table_detection import UpdateFromTableDetection from ada_planning_scene.workspace_walls import WorkspaceWalls +from ada_feeding.helpers import get_tool_joints class ADAPlanningScene(Node): @@ -53,13 +55,25 @@ def __init__(self): """ super().__init__("ada_planning_scene") + try: + # Get the end_effector_tool parameter + self.declare_parameter('end_effector_tool') # Declaring the parameter first + self.end_effector_tool = self.get_parameter('end_effector_tool').value + self.get_logger().info(f"End effector tool: {self.end_effector_tool}") + except ParameterNotDeclaredException: + self.get_logger().warn("Parameter 'end_effector_tool' not declared. Using default value.") + self.end_effector_tool = "fork" # Provide a sensible default + + self.tool_joints = get_tool_joints(self.end_effector_tool) + self.get_logger().info(f"Tool joints: {self.tool_joints}") + # Load the parameters. Note that each of the other classes below may # initialize their own parameters, all of which will be in the same # namespace as the parameters loaded here. self.__load_parameters() # Create an object to add collision objects to the planning scene - self.__collision_object_manager = CollisionObjectManager(node=self) + self.__collision_object_manager = CollisionObjectManager(node=self, tool_joints=self.tool_joints) # Create the initializer self.__initializer = PlanningSceneInitializer( @@ -86,6 +100,7 @@ def __init__(self): tf_buffer=self.__tf_buffer, namespaces=self.__namespaces, namespace_to_use=self.__namespace_to_use, + tool_joints=self.tool_joints ) # Add a callback to update the namespace to use diff --git a/ada_planning_scene/ada_planning_scene/collision_object_manager.py b/ada_planning_scene/ada_planning_scene/collision_object_manager.py index 63878bec..ab4433cd 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -9,7 +9,7 @@ # Standard imports from threading import Lock -from typing import Callable, Dict, Optional, Union +from typing import Callable, Dict, List, Optional, Union # Third-party imports from moveit_msgs.msg import CollisionObject, PlanningScene @@ -36,7 +36,7 @@ class CollisionObjectManager: __GLOBAL_BATCH_ID = "global" __BATCH_ID_FORMAT = "batch_{:d}" - def __init__(self, node: Node): + def __init__(self, node: Node, tool_joints: List): """ Initialize the CollisionObjectManager. @@ -50,9 +50,10 @@ def __init__(self, node: Node): # Using ReentrantCallbackGroup to align with the examples from pymoveit2. # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. callback_group = ReentrantCallbackGroup() + joint_names = kinova.joint_names() + tool_joints self.moveit2 = MoveIt2( node=self.__node, - joint_names=kinova.joint_names(), + joint_names=joint_names, base_link_name=kinova.base_link_name(), end_effector_name="forkTip", group_name="ada", diff --git a/ada_planning_scene/ada_planning_scene/workspace_walls.py b/ada_planning_scene/ada_planning_scene/workspace_walls.py index 3d9f0012..40f68d49 100644 --- a/ada_planning_scene/ada_planning_scene/workspace_walls.py +++ b/ada_planning_scene/ada_planning_scene/workspace_walls.py @@ -71,6 +71,7 @@ def __init__( tf_buffer: Buffer, namespaces: List[str], namespace_to_use: str, + tool_joints: List, ): """ Initialize the workspace walls. @@ -96,9 +97,10 @@ def __init__( self.__tf_buffer = tf_buffer self.__namespaces = namespaces self.__namespace_to_use = namespace_to_use + self.__tool_joints = tool_joints # Load parameters - self.__load_parameters() + self.__load_parameters(self.__tool_joints) # Check if the necessary parameters are set to use the robot model self.__use_robot_model = True @@ -153,7 +155,7 @@ def __init__( self.__active_goal_request_lock = Lock() self.__active_goal_request = None - def __load_parameters(self): + def __load_parameters(self, tool_joints: List): """ Load parameters relevant to the workspace walls. """ @@ -361,7 +363,7 @@ def __load_parameters(self): "j2n6s200_joint_4", "j2n6s200_joint_5", "j2n6s200_joint_6", - ], # default value + ] + tool_joints, # default value ParameterDescriptor( name="articulated_joint_names", type=ParameterType.PARAMETER_STRING_ARRAY, From 7aacc8d3979ccb8aa0bc28bc125580167d0895a7 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:46:06 -0800 Subject: [PATCH 16/26] Pass end_effector_tool parameter to ada_planning_scene --- ada_planning_scene/launch/ada_moveit_launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ada_planning_scene/launch/ada_moveit_launch.xml b/ada_planning_scene/launch/ada_moveit_launch.xml index 2edd9cdd..5d857469 100644 --- a/ada_planning_scene/launch/ada_moveit_launch.xml +++ b/ada_planning_scene/launch/ada_moveit_launch.xml @@ -28,6 +28,7 @@ + From 7e98f91428d6789e0cf1ddd5426c184912a4d194 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 13:48:18 -0800 Subject: [PATCH 17/26] Adjust in_front_of_face_wall to account for length of Articulable Fork --- .../config/ada_planning_scene.yaml | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index 674cf743..e49fda84 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -93,7 +93,7 @@ ada_planning_scene: in_front_of_face_wall: # a wall in front of the user's face so robot motions don't unnecessarily move towards them. primitive_type: 1 # shape_msgs/SolidPrimitive.BOX primitive_dims: [0.65, 0.01, 0.4] - position: [0.37, 0.17, 0.85] + position: [0.37, 0.22, 0.85] quat_xyzw: [0.0, 0.0, 0.0, 1.0] frame_id: root # the frame_id that the pose is relative to add_on_initialize: False # don't add this to the planning scene initially @@ -264,10 +264,20 @@ ada_planning_scene: workspace_walls: seated: workspace_wall_margin_y_min: 0.2 # The min y direction needs a larger margin since the robot moves into that for acquisition - disable_workspace_wall_y_max: True # Remove the back wall, to be able to see in in RVIZ + disable_workspace_wall_y_min: True # Remove the back wall, to be able to see in in RVIZ + disable_workspace_wall_y_max: True + disable_workspace_wall_x_min: True + disable_workspace_wall_x_max: True + disable_workspace_wall_z_min: True + disable_workspace_wall_z_max: True seated_90: workspace_wall_margin_y_min: 0.2 # The min y direction needs a larger margin since the robot moves into that for acquisition - disable_workspace_wall_y_max: True # Remove the back wall, to be able to see in in RVIZ + disable_workspace_wall_y_min: True # Remove the back wall, to be able to see in in RVIZ + disable_workspace_wall_y_max: True + disable_workspace_wall_x_min: True + disable_workspace_wall_x_max: True + disable_workspace_wall_z_min: True + disable_workspace_wall_z_max: True bedside: disable_workspace_wall_y_min: True # Remove the back wall, to be able to see in in RVIZ bedside_90: From b9f3325811c74b9e677585fd4776ae6561a08822 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 14:03:26 -0800 Subject: [PATCH 18/26] Add end_effector_tool argument to moveit and feeding screen sessions --- start.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/start.py b/start.py index 67904aeb..3f1ab19a 100755 --- a/start.py +++ b/start.py @@ -209,12 +209,13 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "feeding": [ ( "ros2 launch ada_feeding ada_feeding_launch.xml use_estop:=false " - f"policy:={args.policy}" + f"policy:={args.policy} " + f"end_effector_tool:={args.end_effector_tool}" ), ], "moveit": [ "ros2 launch ada_planning_scene ada_moveit_launch.xml sim:=mock " - f"end_effector_tool:={args.end_effector_tool}", + f"end_effector_tool:={args.end_effector_tool}" ], "browser": [ "cd ./src/feeding_web_interface/feedingwebapp", @@ -299,7 +300,8 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "sudo ./src/ada_feeding/configure_lovelace.sh", ( "ros2 launch ada_feeding ada_feeding_launch.xml " - f"use_estop:={'false' if args.dev else 'true'} run_web_bridge:=false policy:={args.policy}" + f"use_estop:={'false' if args.dev else 'true'} run_web_bridge:=false policy:={args.policy} " + f"end_effector_tool:={args.end_effector_tool}" ), ], "browser": [ From 1cd735786221e4896074c5ba9ca867d6c097f144 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 17:20:59 -0800 Subject: [PATCH 19/26] Run pre-commit --- ada_feeding/ada_feeding/helpers.py | 7 ++--- ada_feeding/scripts/create_action_servers.py | 28 +++++++++++++------ .../ada_planning_scene/ada_planning_scene.py | 14 ++++++---- .../ada_planning_scene/workspace_walls.py | 3 +- 4 files changed, 34 insertions(+), 18 deletions(-) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index a6007f1f..8735de97 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -362,9 +362,7 @@ def get_moveit2_object( callback_group = ReentrantCallbackGroup() tool_joints = get_tool_joints(blackboard.get(end_effector_tool_blackboard_key)) joint_names = kinova.joint_names() + tool_joints - node.get_logger().info( - f"Creating MoveIt2 object with: {joint_names}" - ) + node.get_logger().info(f"Creating MoveIt2 object with: {joint_names}") moveit2 = MoveIt2( node=node, joint_names=joint_names, @@ -463,6 +461,7 @@ def import_from_string(import_string: str) -> Any: except Exception as exc: raise ImportError(f"Error importing {import_string}") from exc + def get_tool_joints(end_effector_tool: str) -> List[str]: """ Returns a list of joint names associated with the given end-effector tool. @@ -473,7 +472,7 @@ def get_tool_joints(end_effector_tool: str) -> List[str]: Returns: A list of joint names, or an empty list if no joints are associated with the tool. - + Raises: ValueError: If an unsupported tool is provided. """ diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index 152086fc..8cb7eac0 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -138,11 +138,13 @@ def initialize(self, blackboard: py_trees.blackboard.Client) -> None: try: # Get the end_effector_tool parameter - self.declare_parameter('end_effector_tool') # Declaring the parameter first - self.end_effector_tool = self.get_parameter('end_effector_tool').value + self.declare_parameter("end_effector_tool") # Declaring the parameter first + self.end_effector_tool = self.get_parameter("end_effector_tool").value self.get_logger().info(f"End effector tool: {self.end_effector_tool}") except ParameterNotDeclaredException: - self.get_logger().warn("Parameter 'end_effector_tool' not declared. Using default value.") + self.get_logger().warn( + "Parameter 'end_effector_tool' not declared. Using default value." + ) self.end_effector_tool = "fork" # Provide a sensible default self.tool_joints = get_tool_joints(self.end_effector_tool) @@ -150,7 +152,9 @@ def initialize(self, blackboard: py_trees.blackboard.Client) -> None: # Set the end_effector_tool on the global blackboard using the provided client blackboard.register_key("/end_effector_tool", Access.WRITE) - blackboard.set("/end_effector_tool", self.end_effector_tool) # Set the global key + blackboard.set( + "/end_effector_tool", self.end_effector_tool + ) # Set the global key # Read the parameters that specify what action servers to create. self.namespace_to_use = CreateActionServers.DEFAULT_PARAMETER_NAMESPACE @@ -373,14 +377,22 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam else: value = self.parameters[default_namespace][full_name] - if kw.endswith("joint_positions") or kw.endswith("goal_configuration"): + if kw.endswith("joint_positions") or kw.endswith( + "goal_configuration" + ): if isinstance(value, list): # Check if the value is a list zeros_to_append = [0.0] * len(self.tool_joints) value.extend(zeros_to_append) # Append tool joints - self.get_logger().info(f"Appending tool joints to {full_name}: {self.tool_joints}") - self.get_logger().info(f"Value of {full_name} after appending: {value}") + self.get_logger().info( + f"Appending tool joints to {full_name}: {self.tool_joints}" + ) + self.get_logger().info( + f"Value of {full_name} after appending: {value}" + ) else: - self.get_logger().warn(f"tree_kwarg {full_name} is not a list, cannot append tool joints") + self.get_logger().warn( + f"tree_kwarg {full_name} is not a list, cannot append tool joints" + ) tree_kwargs[kw] = value action_server_params[server_name] = ActionServerParams( diff --git a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py index 7100f2a4..4b9f1ad5 100755 --- a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py +++ b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py @@ -57,11 +57,13 @@ def __init__(self): try: # Get the end_effector_tool parameter - self.declare_parameter('end_effector_tool') # Declaring the parameter first - self.end_effector_tool = self.get_parameter('end_effector_tool').value + self.declare_parameter("end_effector_tool") # Declaring the parameter first + self.end_effector_tool = self.get_parameter("end_effector_tool").value self.get_logger().info(f"End effector tool: {self.end_effector_tool}") except ParameterNotDeclaredException: - self.get_logger().warn("Parameter 'end_effector_tool' not declared. Using default value.") + self.get_logger().warn( + "Parameter 'end_effector_tool' not declared. Using default value." + ) self.end_effector_tool = "fork" # Provide a sensible default self.tool_joints = get_tool_joints(self.end_effector_tool) @@ -73,7 +75,9 @@ def __init__(self): self.__load_parameters() # Create an object to add collision objects to the planning scene - self.__collision_object_manager = CollisionObjectManager(node=self, tool_joints=self.tool_joints) + self.__collision_object_manager = CollisionObjectManager( + node=self, tool_joints=self.tool_joints + ) # Create the initializer self.__initializer = PlanningSceneInitializer( @@ -100,7 +104,7 @@ def __init__(self): tf_buffer=self.__tf_buffer, namespaces=self.__namespaces, namespace_to_use=self.__namespace_to_use, - tool_joints=self.tool_joints + tool_joints=self.tool_joints, ) # Add a callback to update the namespace to use diff --git a/ada_planning_scene/ada_planning_scene/workspace_walls.py b/ada_planning_scene/ada_planning_scene/workspace_walls.py index d0d7862e..58205e56 100644 --- a/ada_planning_scene/ada_planning_scene/workspace_walls.py +++ b/ada_planning_scene/ada_planning_scene/workspace_walls.py @@ -363,7 +363,8 @@ def __load_parameters(self, tool_joints: List): "j2n6s200_joint_4", "j2n6s200_joint_5", "j2n6s200_joint_6", - ] + tool_joints, # default value + ] + + tool_joints, # default value ParameterDescriptor( name="articulated_joint_names", type=ParameterType.PARAMETER_STRING_ARRAY, From 1e94a5d90e08256572d1a572c3d0ee577f320eaf Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 14 Jan 2025 12:30:06 -0800 Subject: [PATCH 20/26] Revert changes to disable_workspace_walls --- ada_planning_scene/config/ada_planning_scene.yaml | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index cfa7fe11..b63702d4 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -264,20 +264,10 @@ ada_planning_scene: workspace_walls: seated: workspace_wall_margin_y_min: 0.2 # The min y direction needs a larger margin since the robot moves into that for acquisition - disable_workspace_wall_y_min: True # Remove the back wall, to be able to see in in RVIZ - disable_workspace_wall_y_max: True - disable_workspace_wall_x_min: True - disable_workspace_wall_x_max: True - disable_workspace_wall_z_min: True - disable_workspace_wall_z_max: True + disable_workspace_wall_y_max: True # Remove the back wall, to be able to see in in RVIZ seated_90: workspace_wall_margin_y_min: 0.2 # The min y direction needs a larger margin since the robot moves into that for acquisition - disable_workspace_wall_y_min: True # Remove the back wall, to be able to see in in RVIZ - disable_workspace_wall_y_max: True - disable_workspace_wall_x_min: True - disable_workspace_wall_x_max: True - disable_workspace_wall_z_min: True - disable_workspace_wall_z_max: True + disable_workspace_wall_y_max: True # Remove the back wall, to be able to see in in RVIZ bedside: disable_workspace_wall_y_min: True # Remove the back wall, to be able to see in in RVIZ bedside_90: From 59fbbab3d16d5998d2fd1d865b8403b64af14f73 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 15 Jan 2025 09:25:09 -0800 Subject: [PATCH 21/26] Update choices for end_effector_tool --- ada_feeding/launch/ada_feeding_launch.xml | 2 +- ada_planning_scene/launch/ada_moveit_launch.xml | 2 +- start.py | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ada_feeding/launch/ada_feeding_launch.xml b/ada_feeding/launch/ada_feeding_launch.xml index e7a2a680..f8e792ce 100644 --- a/ada_feeding/launch/ada_feeding_launch.xml +++ b/ada_feeding/launch/ada_feeding_launch.xml @@ -9,7 +9,7 @@ - + diff --git a/ada_planning_scene/launch/ada_moveit_launch.xml b/ada_planning_scene/launch/ada_moveit_launch.xml index 994c2fe4..6842d15a 100644 --- a/ada_planning_scene/launch/ada_moveit_launch.xml +++ b/ada_planning_scene/launch/ada_moveit_launch.xml @@ -10,7 +10,7 @@ - + diff --git a/start.py b/start.py index 3f1ab19a..ca8af3c8 100755 --- a/start.py +++ b/start.py @@ -74,7 +74,8 @@ parser.add_argument( "--end_effector_tool", default="fork", - help=("Which end-effector tool to use: 'none', 'fork', 'articulable_fork'"), + help=("Which end-effector tool to use"), + choices=['fork', 'articulable_fork'], ) From 1005bc8a9351116bc9dad755c645c89e180221d5 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 15 Jan 2025 09:31:28 -0800 Subject: [PATCH 22/26] Change quote character --- start.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/start.py b/start.py index ca8af3c8..5205a458 100755 --- a/start.py +++ b/start.py @@ -75,7 +75,7 @@ "--end_effector_tool", default="fork", help=("Which end-effector tool to use"), - choices=['fork', 'articulable_fork'], + choices=["fork", "articulable_fork"], ) From dbbcaae7ba129a0fc2188894fd673cde3d120c47 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 15 Jan 2025 10:36:39 -0800 Subject: [PATCH 23/26] Address pylint warnings --- ada_feeding/ada_feeding/helpers.py | 4 ++-- ada_feeding/scripts/create_action_servers.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 8735de97..5f474d8c 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -485,5 +485,5 @@ def get_tool_joints(end_effector_tool: str) -> List[str]: if end_effector_tool in tool_joints: return tool_joints[end_effector_tool] - else: - raise ValueError(f"Unknown end_effector_tool: {end_effector_tool}") + + raise ValueError(f"Unknown end_effector_tool: {end_effector_tool}") diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index 8cb7eac0..bfdc79f4 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -197,7 +197,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam ------- action_server_params: A dict mapping server names to ActionServerParams objects. """ - # pylint: disable=too-many-locals + # pylint: disable=too-many-locals, too-many-branches # Okay because we are providing a lot of generic capabilities through parameters default_namespace = CreateActionServers.DEFAULT_PARAMETER_NAMESPACE From f6b89a5bd56ec41daa70eef7d6b952a99f4318c6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 17 Feb 2025 17:16:04 -0800 Subject: [PATCH 24/26] Restore comments detailing joint names for configuration --- .../config/ada_feeding_action_servers_default.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_default.yaml b/ada_feeding/config/ada_feeding_action_servers_default.yaml index d86dcc1c..ead904b7 100644 --- a/ada_feeding/config/ada_feeding_action_servers_default.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_default.yaml @@ -39,12 +39,12 @@ ada_feeding_action_servers: - max_velocity_scaling_factor tree_kwargs: # optional joint_positions: # required - - -2.4538579336877304 - - 3.07974419938212 - - 1.8320725365979 - - 4.096143890468605 - - -2.003422584820525 - - -3.2123560395465063 + - -2.4538579336877304 # j2n6s200_joint_1 + - 3.07974419938212 # j2n6s200_joint_2 + - 1.8320725365979 # j2n6s200_joint_3 + - 4.096143890468605 # j2n6s200_joint_4 + - -2.003422584820525 # j2n6s200_joint_5 + - -3.2123560395465063 # j2n6s200_joint_6 toggle_watchdog_listener: false # optional, default: true f_mag: 4.0 # N max_velocity_scaling_factor: 1.0 # optional in (0.0, 1.0], default: 0.1 From 96af0281bc60d8df304d7f48a85277857210b7ab Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 17 Feb 2025 18:16:38 -0800 Subject: [PATCH 25/26] Add default value for end_effector_tool if not set on blackboard --- ada_feeding/ada_feeding/helpers.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 5f474d8c..f7cce1f6 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -324,6 +324,14 @@ def get_moveit2_object( Raises ------- KeyError: if the MoveIt2 object does not exist and node is None. + + Expects + ------- + The blackboard key `/end_effector_tool` to be set. If this key is not set, + the default value "fork" is used. Other possible values are "none" and + "articulable_fork". It is crucial that the `get_tool_joints` function + can handle these string values. If `/end_effector_tool` is not set, a + warning message will be logged. """ # These Blackboard keys are used to store the single, global MoveIt2 object # and its corresponding lock. Note that it is important that these keys start with @@ -358,6 +366,13 @@ def get_moveit2_object( ) blackboard.register_key(moveit2_blackboard_key, Access.WRITE) blackboard.register_key(moveit2_lock_blackboard_key, Access.WRITE) + blackboard.register_key(end_effector_tool_blackboard_key, Access.WRITE) + end_effector_tool = "fork" # Assign to default value + if blackboard.get(end_effector_tool_blackboard_key) is None: # Check if not set + node.get_logger().warn(f"end_effector_tool not set, using default: {end_effector_tool}") + blackboard.set(end_effector_tool_blackboard_key, end_effector_tool) #Set the value on the blackboard + else: + end_effector_tool = blackboard.get(end_effector_tool_blackboard_key) #Get the value from the blackboard # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. callback_group = ReentrantCallbackGroup() tool_joints = get_tool_joints(blackboard.get(end_effector_tool_blackboard_key)) From 6cf5040f80f7acc36ef7b9e431970599ea307923 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 17 Feb 2025 18:20:44 -0800 Subject: [PATCH 26/26] Run pre-commit --- ada_feeding/ada_feeding/helpers.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index f7cce1f6..6e1d93ec 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -367,12 +367,18 @@ def get_moveit2_object( blackboard.register_key(moveit2_blackboard_key, Access.WRITE) blackboard.register_key(moveit2_lock_blackboard_key, Access.WRITE) blackboard.register_key(end_effector_tool_blackboard_key, Access.WRITE) - end_effector_tool = "fork" # Assign to default value + end_effector_tool = "fork" # Assign to default value if blackboard.get(end_effector_tool_blackboard_key) is None: # Check if not set - node.get_logger().warn(f"end_effector_tool not set, using default: {end_effector_tool}") - blackboard.set(end_effector_tool_blackboard_key, end_effector_tool) #Set the value on the blackboard + node.get_logger().warn( + f"end_effector_tool not set, using default: {end_effector_tool}" + ) + blackboard.set( + end_effector_tool_blackboard_key, end_effector_tool + ) # Set the value on the blackboard else: - end_effector_tool = blackboard.get(end_effector_tool_blackboard_key) #Get the value from the blackboard + end_effector_tool = blackboard.get( + end_effector_tool_blackboard_key + ) # Get the value from the blackboard # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. callback_group = ReentrantCallbackGroup() tool_joints = get_tool_joints(blackboard.get(end_effector_tool_blackboard_key))