From d9520a9898b2897c70acc6576c61e86f02f879b5 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 21 Dec 2023 16:53:16 -0800 Subject: [PATCH 1/7] Made tree_kwargs changeable while keeping defaults --- .pylintrc | 1 + .../ada_feeding/trees/move_to_mouth_tree.py | 4 +- .../config/ada_feeding_action_servers.yaml | 298 ----------------- .../ada_feeding_action_servers_current.yaml | 6 + .../ada_feeding_action_servers_default.yaml | 305 ++++++++++++++++++ ada_feeding/launch/ada_feeding_launch.xml | 3 +- ada_feeding/scripts/create_action_servers.py | 177 ++++++++-- 7 files changed, 471 insertions(+), 323 deletions(-) delete mode 100644 ada_feeding/config/ada_feeding_action_servers.yaml create mode 100644 ada_feeding/config/ada_feeding_action_servers_current.yaml create mode 100644 ada_feeding/config/ada_feeding_action_servers_default.yaml diff --git a/.pylintrc b/.pylintrc index c9c326d2..22ccac2c 100644 --- a/.pylintrc +++ b/.pylintrc @@ -212,6 +212,7 @@ good-names=a, ax, ex, hz, + kw, ns, Run, _ diff --git a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py index bbbddedb..fab0556e 100644 --- a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py @@ -10,7 +10,7 @@ # Standard imports import os -from typing import Tuple +from typing import List, Tuple # Third-party imports from ament_index_python.packages import get_package_share_directory @@ -90,7 +90,7 @@ def __init__( allowed_face_distance: Tuple[float, float] = (0.4, 1.25), face_detection_msg_timeout: float = 5.0, face_detection_timeout: float = 2.5, - plan_distance_from_mouth: Tuple[float, float, float] = (0.025, 0.0, -0.01), + plan_distance_from_mouth: List[float] = [0.025, 0.0, -0.01], fork_target_orientation_from_mouth: Tuple[float, float, float, float] = ( 0.5, -0.5, diff --git a/ada_feeding/config/ada_feeding_action_servers.yaml b/ada_feeding/config/ada_feeding_action_servers.yaml deleted file mode 100644 index 54986603..00000000 --- a/ada_feeding/config/ada_feeding_action_servers.yaml +++ /dev/null @@ -1,298 +0,0 @@ -# NOTE: You have to change this node name if you change the node name in the launchfile. -ada_feeding_action_servers: - ros__parameters: - # If we haven't received a message from the watchdog in this time, stop all robot motions - watchdog_timeout_sec: 0.5 # sec - - # A list of the names of the action servers to create. Each one must have - # it's own parameters (below) specifying action_type and tree_class. - server_names: - - MoveAbovePlate - - AcquireFood - - MoveToStagingConfiguration - - MoveToMouth - - MoveFromMouthToStagingConfiguration - - MoveFromMouthToAbovePlate - - MoveFromMouthToRestingPosition - - MoveToRestingPosition - - MoveToStowLocation - - TestMoveToPose - - StartServo - - StopServo - - # Parameters for the MoveAbovePlate action - MoveAbovePlate: # required - action_type: ada_feeding_msgs.action.MoveTo # required - tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required - tree_kws: # optional - - joint_positions - - toggle_watchdog_listener - - f_mag - - max_velocity_scaling_factor - tree_kwargs: # optional - joint_positions: # required - - -2.11666 # j2n6s200_joint_1 - - 3.34967 # j2n6s200_joint_2 - - 2.04129 # j2n6s200_joint_3 - - -2.30031 # j2n6s200_joint_4 - - -2.34026 # j2n6s200_joint_5 - - 2.95450 # j2n6s200_joint_6 - toggle_watchdog_listener: false # optional, default: true - f_mag: 4.0 # N - max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the AcquireFood action - AcquireFood: # required - action_type: ada_feeding_msgs.action.AcquireFood # required - tree_class: ada_feeding.trees.AcquireFoodTree # required - tree_kws: - - resting_joint_positions - tree_kwargs: - resting_joint_positions: # required - - -1.94672 # j2n6s200_joint_1 - - 2.51268 # j2n6s200_joint_2 - - 0.35653 # j2n6s200_joint_3 - - -4.76501 # j2n6s200_joint_4 - - 5.99991 # j2n6s200_joint_5 - - 4.99555 # j2n6s200_joint_6 - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the MoveToRestingPosition action - MoveToRestingPosition: # required - action_type: ada_feeding_msgs.action.MoveTo # required - tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required - tree_kws: # optional - - joint_positions - - toggle_watchdog_listener - - f_mag - - max_velocity_scaling_factor - tree_kwargs: # optional - joint_positions: # required - - -1.94672 # j2n6s200_joint_1 - - 2.51268 # j2n6s200_joint_2 - - 0.35653 # j2n6s200_joint_3 - - -4.76501 # j2n6s200_joint_4 - - 5.99991 # j2n6s200_joint_5 - - 4.99555 # j2n6s200_joint_6 - toggle_watchdog_listener: false # optional, default: true - f_mag: 4.0 # N - max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the MoveToMouth action - MoveToStagingConfiguration: # required - action_type: ada_feeding_msgs.action.MoveTo # required - tree_class: ada_feeding.trees.MoveToConfigurationWithWheelchairWallTree # required - tree_kws: # optional - - goal_configuration - # - orientation_constraint_quaternion - # - orientation_constraint_tolerances - - allowed_planning_time - - max_velocity_scaling_factor - tree_kwargs: # optional - goal_configuration: - # # Side-staging configuration - # - 2.74709 # j2n6s200_joint_1 - # - 2.01400 # j2n6s200_joint_2 - # - 1.85257 # j2n6s200_joint_3 - # - -2.83523 # j2n6s200_joint_4 - # - -1.61925 # j2n6s200_joint_5 - # - -2.67325 # j2n6s200_joint_6 - # # Front-staging configuration -- Taller - # - -2.30252 - # - 4.23221 - # - 3.84109 - # - -4.65546 - # - 3.94225 - # - 4.26543 - # Front-staging configuration -- Shorter - - -2.32526 - - 4.456298 - - 4.16769 - - 1.53262 - - -2.18359 - - -2.19525 - orientation_constraint_quaternion: # perpendicular to the base link - - 0.707168 # x - - 0.0 # y - - 0.0 # z - - 0.707168 # w - orientation_constraint_tolerances: # Note that tolerances are w.r.t. the axes in quat_xyzw_path - - 0.6 # x, rad - - 3.14159 # y, rad - - 0.5 # z, rad - allowed_planning_time: 1.0 # secs - max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the MoveToMouth action - MoveToMouth: # required - action_type: ada_feeding_msgs.action.MoveToMouth # required - tree_class: ada_feeding.trees.MoveToMouthTree # required - tree_kws: # optional - - face_detection_msg_timeout - - face_detection_timeout - - fork_target_orientation_from_mouth - tree_kwargs: # optional - face_detection_msg_timeout: 10.0 # sec - face_detection_timeout: 5.0 # sec - fork_target_orientation_from_mouth: # In mouth frame, +x out of mouth, +z towards top of head - - 0.5751716 - - -0.5751716 - - -0.4113121 - - 0.4113121 - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the MoveFromMouthToStagingConfiguration action. - # No need for orientation path constraints when moving above the plate - # because presumably there is no food on the fork. - MoveFromMouthToStagingConfiguration: # required - action_type: ada_feeding_msgs.action.MoveTo # required - tree_class: ada_feeding.trees.MoveFromMouthTree # required - tree_kws: # optional - - staging_configuration_position - - staging_configuration_quat_xyzw - tree_kwargs: # optional - staging_configuration_position: - - 0.28323 # x - - 0.07289 # y - - 0.69509 # z - staging_configuration_quat_xyzw: - - 0.00472 # x - - 0.69757 # y - - 0.71645 # z - - -0.00811 # w - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the MoveFromMouthToAbovePlate action. - # No need for orientation path constraints when moving above the plate - # because presumably there is no food on the fork. - MoveFromMouthToAbovePlate: # required - action_type: ada_feeding_msgs.action.MoveTo # required - tree_class: ada_feeding.trees.MoveFromMouthTree # required - tree_kws: # optional - - staging_configuration_position - - staging_configuration_quat_xyzw - - end_configuration - - max_velocity_scaling_factor_to_end_configuration - tree_kwargs: # optional - staging_configuration_position: - - 0.28323 # x - - 0.07289 # y - - 0.69509 # z - staging_configuration_quat_xyzw: - - 0.00472 # x - - 0.69757 # y - - 0.71645 # z - - -0.00811 # w - end_configuration: - - -2.11666 # j2n6s200_joint_1 - - 3.34967 # j2n6s200_joint_2 - - 2.04129 # j2n6s200_joint_3 - - -2.30031 # j2n6s200_joint_4 - - -2.34026 # j2n6s200_joint_5 - - 2.95450 # j2n6s200_joint_6 - max_velocity_scaling_factor_to_end_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1 - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the MoveFromMouthToRestingPosition action. - # Need orientation path constraints to end config because - # presumably there is still food on the fork. - MoveFromMouthToRestingPosition: # required - action_type: ada_feeding_msgs.action.MoveTo # required - tree_class: ada_feeding.trees.MoveFromMouthTree # required - tree_kws: # optional - - staging_configuration_position - - staging_configuration_quat_xyzw - - end_configuration - # - orientation_constraint_to_end_configuration_quaternion - # - orientation_constraint_to_end_configuration_tolerances - - allowed_planning_time_to_end_configuration - - max_velocity_scaling_factor_to_end_configuration - tree_kwargs: # optional - staging_configuration_position: - - 0.28323 # x - - 0.07289 # y - - 0.69509 # z - staging_configuration_quat_xyzw: - - 0.00472 # x - - 0.69757 # y - - 0.71645 # z - - -0.00811 # w - end_configuration: - - -1.94672 # j2n6s200_joint_1 - - 2.51268 # j2n6s200_joint_2 - - 0.35653 # j2n6s200_joint_3 - - -4.76501 # j2n6s200_joint_4 - - 5.99991 # j2n6s200_joint_5 - - 4.99555 # j2n6s200_joint_6 - orientation_constraint_to_end_configuration_quaternion: # perpendicular to the base link - - 0.707168 # x - - 0.0 # y - - 0.0 # z - - 0.707168 # w - orientation_constraint_to_end_configuration_tolerances: # Note that tolerances are w.r.t. the axes in quat_xyzw_path - - 0.6 # x, rad - - 3.14159 # y, rad - - 0.5 # z, rad - max_velocity_scaling_factor_to_end_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1 - allowed_planning_time_to_end_configuration: 1.0 # secs - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the MoveToStowLocation action - # TODO: We may be able to get a stow location that actually - # tucks to the side of the user's wheelchair. - MoveToStowLocation: # required - action_type: ada_feeding_msgs.action.MoveTo # required - tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required - tree_kws: # optional - - joint_positions - - toggle_watchdog_listener - - f_mag - - max_velocity_scaling_factor - tree_kwargs: # optional - joint_positions: # required - - -1.52101 # j2n6s200_joint_1 - - 2.60098 # j2n6s200_joint_2 - - 0.32811 # j2n6s200_joint_3 - - -4.00012 # j2n6s200_joint_4 - - 0.22831 # j2n6s200_joint_5 - - 3.87886 # j2n6s200_joint_6 - toggle_watchdog_listener: false # optional, default: true - f_mag: 4.0 # N - max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the TestMoveToPose action. Moves to the staging location - TestMoveToPose: # required - action_type: ada_feeding_msgs.action.MoveTo # required - tree_class: ada_feeding.trees.MoveToPoseTree # required - tree_kws: # optional - - position - - quat_xyzw - - max_velocity_scaling_factor - tree_kwargs: # optional - position: - - 0.28323 # x - - 0.07289 # y - - 0.69509 # z - quat_xyzw: - - 0.00472 # x - - 0.69757 # y - - 0.71645 # z - - -0.00811 # w - max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the StartServo action - StartServo: # required - action_type: ada_feeding_msgs.action.Trigger # required - tree_class: ada_feeding.trees.StartServoTree # required - tick_rate: 10 # Hz, optional, default: 30 - - # Parameters for the StartServo action - StopServo: # required - action_type: ada_feeding_msgs.action.Trigger # required - tree_class: ada_feeding.trees.StopServoTree # required - tick_rate: 10 # Hz, optional, default: 30 diff --git a/ada_feeding/config/ada_feeding_action_servers_current.yaml b/ada_feeding/config/ada_feeding_action_servers_current.yaml new file mode 100644 index 00000000..f24158b2 --- /dev/null +++ b/ada_feeding/config/ada_feeding_action_servers_current.yaml @@ -0,0 +1,6 @@ +# This file is auto-generated by create_action_servers.py +ada_feeding_action_servers: + ros__parameters: + current: + overridden_parameters: + - "" diff --git a/ada_feeding/config/ada_feeding_action_servers_default.yaml b/ada_feeding/config/ada_feeding_action_servers_default.yaml new file mode 100644 index 00000000..6e433881 --- /dev/null +++ b/ada_feeding/config/ada_feeding_action_servers_default.yaml @@ -0,0 +1,305 @@ +# NOTE: You have to change this node name if you change the node name in the launchfile. +ada_feeding_action_servers: + ros__parameters: + default: + # If we haven't received a message from the watchdog in this time, stop all robot motions + watchdog_timeout_sec: 0.5 # sec + + # A list of the names of the action servers to create. Each one must have + # it's own parameters (below) specifying action_type and tree_class. + server_names: + - MoveAbovePlate + - AcquireFood + - MoveToStagingConfiguration + - MoveToMouth + - MoveFromMouthToStagingConfiguration + - MoveFromMouthToAbovePlate + - MoveFromMouthToRestingPosition + - MoveToRestingPosition + - MoveToStowLocation + - TestMoveToPose + - StartServo + - StopServo + + # Parameters for the MoveAbovePlate action + MoveAbovePlate: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required + tree_kws: # optional + - joint_positions + - toggle_watchdog_listener + - f_mag + - max_velocity_scaling_factor + tree_kwargs: # optional + joint_positions: # required + - -2.11666 # j2n6s200_joint_1 + - 3.34967 # j2n6s200_joint_2 + - 2.04129 # j2n6s200_joint_3 + - -2.30031 # j2n6s200_joint_4 + - -2.34026 # j2n6s200_joint_5 + - 2.95450 # j2n6s200_joint_6 + toggle_watchdog_listener: false # optional, default: true + f_mag: 4.0 # N + max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the AcquireFood action + AcquireFood: # required + action_type: ada_feeding_msgs.action.AcquireFood # required + tree_class: ada_feeding.trees.AcquireFoodTree # required + tree_kws: + - resting_joint_positions + tree_kwargs: + resting_joint_positions: # required + - -1.94672 # j2n6s200_joint_1 + - 2.51268 # j2n6s200_joint_2 + - 0.35653 # j2n6s200_joint_3 + - -4.76501 # j2n6s200_joint_4 + - 5.99991 # j2n6s200_joint_5 + - 4.99555 # j2n6s200_joint_6 + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveToRestingPosition action + MoveToRestingPosition: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required + tree_kws: # optional + - joint_positions + - toggle_watchdog_listener + - f_mag + - max_velocity_scaling_factor + tree_kwargs: # optional + joint_positions: # required + - -1.94672 # j2n6s200_joint_1 + - 2.51268 # j2n6s200_joint_2 + - 0.35653 # j2n6s200_joint_3 + - -4.76501 # j2n6s200_joint_4 + - 5.99991 # j2n6s200_joint_5 + - 4.99555 # j2n6s200_joint_6 + toggle_watchdog_listener: false # optional, default: true + f_mag: 4.0 # N + max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveToMouth action + MoveToStagingConfiguration: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveToConfigurationWithWheelchairWallTree # required + tree_kws: # optional + - goal_configuration + # - orientation_constraint_quaternion + # - orientation_constraint_tolerances + - allowed_planning_time + - max_velocity_scaling_factor + tree_kwargs: # optional + goal_configuration: + # # Side-staging configuration + # - 2.74709 # j2n6s200_joint_1 + # - 2.01400 # j2n6s200_joint_2 + # - 1.85257 # j2n6s200_joint_3 + # - -2.83523 # j2n6s200_joint_4 + # - -1.61925 # j2n6s200_joint_5 + # - -2.67325 # j2n6s200_joint_6 + # # Front-staging configuration -- Taller + # - -2.30252 + # - 4.23221 + # - 3.84109 + # - -4.65546 + # - 3.94225 + # - 4.26543 + # Front-staging configuration -- Shorter + - -2.32526 + - 4.456298 + - 4.16769 + - 1.53262 + - -2.18359 + - -2.19525 + orientation_constraint_quaternion: # perpendicular to the base link + - 0.707168 # x + - 0.0 # y + - 0.0 # z + - 0.707168 # w + orientation_constraint_tolerances: # Note that tolerances are w.r.t. the axes in quat_xyzw_path + - 0.6 # x, rad + - 3.14159 # y, rad + - 0.5 # z, rad + allowed_planning_time: 1.0 # secs + max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveToMouth action + MoveToMouth: # required + action_type: ada_feeding_msgs.action.MoveToMouth # required + tree_class: ada_feeding.trees.MoveToMouthTree # required + tree_kws: # optional + - face_detection_msg_timeout + - face_detection_timeout + - fork_target_orientation_from_mouth + - plan_distance_from_mouth + + tree_kwargs: # optional + face_detection_msg_timeout: 10.0 # sec + face_detection_timeout: 5.0 # sec + fork_target_orientation_from_mouth: # In mouth frame, +x out of mouth, +z towards top of head + - 0.5751716 + - -0.5751716 + - -0.4113121 + - 0.4113121 + plan_distance_from_mouth: + - 0.025 # m + - 0.0 # m + - -0.01 # m + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveFromMouthToStagingConfiguration action. + # No need for orientation path constraints when moving above the plate + # because presumably there is no food on the fork. + MoveFromMouthToStagingConfiguration: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveFromMouthTree # required + tree_kws: # optional + - staging_configuration_position + - staging_configuration_quat_xyzw + tree_kwargs: # optional + staging_configuration_position: + - 0.28323 # x + - 0.07289 # y + - 0.69509 # z + staging_configuration_quat_xyzw: + - 0.00472 # x + - 0.69757 # y + - 0.71645 # z + - -0.00811 # w + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveFromMouthToAbovePlate action. + # No need for orientation path constraints when moving above the plate + # because presumably there is no food on the fork. + MoveFromMouthToAbovePlate: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveFromMouthTree # required + tree_kws: # optional + - staging_configuration_position + - staging_configuration_quat_xyzw + - end_configuration + - max_velocity_scaling_factor_to_end_configuration + tree_kwargs: # optional + staging_configuration_position: + - 0.28323 # x + - 0.07289 # y + - 0.69509 # z + staging_configuration_quat_xyzw: + - 0.00472 # x + - 0.69757 # y + - 0.71645 # z + - -0.00811 # w + end_configuration: + - -2.11666 # j2n6s200_joint_1 + - 3.34967 # j2n6s200_joint_2 + - 2.04129 # j2n6s200_joint_3 + - -2.30031 # j2n6s200_joint_4 + - -2.34026 # j2n6s200_joint_5 + - 2.95450 # j2n6s200_joint_6 + max_velocity_scaling_factor_to_end_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1 + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveFromMouthToRestingPosition action. + # Need orientation path constraints to end config because + # presumably there is still food on the fork. + MoveFromMouthToRestingPosition: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveFromMouthTree # required + tree_kws: # optional + - staging_configuration_position + - staging_configuration_quat_xyzw + - end_configuration + # - orientation_constraint_to_end_configuration_quaternion + # - orientation_constraint_to_end_configuration_tolerances + - allowed_planning_time_to_end_configuration + - max_velocity_scaling_factor_to_end_configuration + tree_kwargs: # optional + staging_configuration_position: + - 0.28323 # x + - 0.07289 # y + - 0.69509 # z + staging_configuration_quat_xyzw: + - 0.00472 # x + - 0.69757 # y + - 0.71645 # z + - -0.00811 # w + end_configuration: + - -1.94672 # j2n6s200_joint_1 + - 2.51268 # j2n6s200_joint_2 + - 0.35653 # j2n6s200_joint_3 + - -4.76501 # j2n6s200_joint_4 + - 5.99991 # j2n6s200_joint_5 + - 4.99555 # j2n6s200_joint_6 + orientation_constraint_to_end_configuration_quaternion: # perpendicular to the base link + - 0.707168 # x + - 0.0 # y + - 0.0 # z + - 0.707168 # w + orientation_constraint_to_end_configuration_tolerances: # Note that tolerances are w.r.t. the axes in quat_xyzw_path + - 0.6 # x, rad + - 3.14159 # y, rad + - 0.5 # z, rad + max_velocity_scaling_factor_to_end_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1 + allowed_planning_time_to_end_configuration: 1.0 # secs + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the MoveToStowLocation action + # TODO: We may be able to get a stow location that actually + # tucks to the side of the user's wheelchair. + MoveToStowLocation: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveToConfigurationWithFTThresholdsTree # required + tree_kws: # optional + - joint_positions + - toggle_watchdog_listener + - f_mag + - max_velocity_scaling_factor + tree_kwargs: # optional + joint_positions: # required + - -1.52101 # j2n6s200_joint_1 + - 2.60098 # j2n6s200_joint_2 + - 0.32811 # j2n6s200_joint_3 + - -4.00012 # j2n6s200_joint_4 + - 0.22831 # j2n6s200_joint_5 + - 3.87886 # j2n6s200_joint_6 + toggle_watchdog_listener: false # optional, default: true + f_mag: 4.0 # N + max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the TestMoveToPose action. Moves to the staging location + TestMoveToPose: # required + action_type: ada_feeding_msgs.action.MoveTo # required + tree_class: ada_feeding.trees.MoveToPoseTree # required + tree_kws: # optional + - position + - quat_xyzw + - max_velocity_scaling_factor + tree_kwargs: # optional + position: + - 0.28323 # x + - 0.07289 # y + - 0.69509 # z + quat_xyzw: + - 0.00472 # x + - 0.69757 # y + - 0.71645 # z + - -0.00811 # w + max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the StartServo action + StartServo: # required + action_type: ada_feeding_msgs.action.Trigger # required + tree_class: ada_feeding.trees.StartServoTree # required + tick_rate: 10 # Hz, optional, default: 30 + + # Parameters for the StartServo action + StopServo: # required + action_type: ada_feeding_msgs.action.Trigger # required + tree_class: ada_feeding.trees.StopServoTree # required + tick_rate: 10 # Hz, optional, default: 30 diff --git a/ada_feeding/launch/ada_feeding_launch.xml b/ada_feeding/launch/ada_feeding_launch.xml index 170946cb..1e488e6b 100644 --- a/ada_feeding/launch/ada_feeding_launch.xml +++ b/ada_feeding/launch/ada_feeding_launch.xml @@ -20,7 +20,8 @@ - + + diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index a46b5ae8..e2faca31 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -5,21 +5,25 @@ """ # Standard imports +import collections.abc +import os import threading import traceback from typing import Any, Awaitable, Callable, Dict, List, Optional, Tuple # Third-party imports from ada_watchdog_listener import ADAWatchdogListener +from ament_index_python.packages import get_package_share_directory import py_trees from py_trees.visitors import DebugVisitor -from rcl_interfaces.msg import ParameterDescriptor, ParameterType +from rcl_interfaces.msg import ParameterDescriptor, ParameterType, SetParametersResult import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse from rclpy.action.server import ServerGoalHandle from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.parameter import Parameter +import yaml # Local imports from ada_feeding import ActionServerBT @@ -91,7 +95,8 @@ def __init__(self) -> None: super().__init__("create_action_servers") # Read the parameters that specify what action servers to create. - action_server_params = self.read_params() + self.action_server_params = self.read_params() + self.add_on_set_parameters_callback(self.parameter_callback) # Create the watchdog listener. Note that this watchdog listener # adds additional parameters -- `watchdog_timeout_sec` and @@ -103,7 +108,7 @@ def __init__(self) -> None: self.active_goal_request = None # Create the action servers. - self.create_action_servers(action_server_params) + self.create_action_servers(self.action_server_params) def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: """ @@ -115,7 +120,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: """ # Read the server names server_names = self.declare_parameter( - "server_names", + "default.server_names", descriptor=ParameterDescriptor( name="server_names", type=ParameterType.PARAMETER_STRING_ARRAY, @@ -127,6 +132,23 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: ), ) + # Read the parameters that have been changed from their default values + overridden_parameters = self.declare_parameter( + "current.overridden_parameters", + descriptor=ParameterDescriptor( + name="overridden_parameters", + type=ParameterType.PARAMETER_STRING_ARRAY, + description=( + "List of parameters that have been changed from their default values. " + "These parameters must be set in the `current` param namespace." + ), + read_only=True, + ), + ) + overridden_parameters = overridden_parameters.value + self.overridden_parameters = {} + self.default_parameters = {} + # Read each action server's params action_server_params = [] for server_name in server_names.value: @@ -135,7 +157,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: "", [ ( - f"{server_name}.action_type", + f"default.{server_name}.action_type", None, ParameterDescriptor( name="action_type", @@ -148,7 +170,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: ), ), ( - f"{server_name}.tree_class", + f"default.{server_name}.tree_class", None, ParameterDescriptor( name="tree_class", @@ -161,7 +183,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: ), ), ( - f"{server_name}.tick_rate", + f"default.{server_name}.tick_rate", 30, ParameterDescriptor( name="tick_rate", @@ -176,7 +198,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: ], ) tree_kws = self.declare_parameter( - f"{server_name}.tree_kws", + f"default.{server_name}.tree_kws", descriptor=ParameterDescriptor( name="tree_kws", type=ParameterType.PARAMETER_STRING_ARRAY, @@ -188,9 +210,11 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: ), ) if tree_kws.value is not None: - tree_kwargs = { - kw: self.declare_parameter( - f"{server_name}.tree_kwargs.{kw}", + tree_kwargs = {} + for kw in tree_kws.value: + full_name = f"{server_name}.tree_kwargs.{kw}" + default_value = self.declare_parameter( + f"default.{full_name}", descriptor=ParameterDescriptor( name=kw, description="Custom keyword argument for the behavior tree.", @@ -198,8 +222,20 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: read_only=True, ), ) - for kw in tree_kws.value - } + self.default_parameters[full_name] = default_value.value + current_value = self.declare_parameter( + f"current.{full_name}", + descriptor=ParameterDescriptor( + name=kw, + description="Custom keyword argument for the behavior tree.", + dynamic_typing=True, + ), + ) + if full_name in overridden_parameters: + tree_kwargs[kw] = current_value.value + self.overridden_parameters[full_name] = current_value.value + else: + tree_kwargs[kw] = default_value.value else: tree_kwargs = {} @@ -215,13 +251,87 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: server_name=server_name, action_type=action_type.value, tree_class=tree_class.value, - tree_kwargs={kw: arg.value for kw, arg in tree_kwargs.items()}, + tree_kwargs=tree_kwargs, tick_rate=tick_rate.value, ) ) return action_server_params + def parameter_callback(self, params: List[Parameter]) -> SetParametersResult: + """ + Callback function for when a parameter is changed. Note that in practice, + only tree_kwargs are not read-only, so we only expect those to be changed. + + Note that we only return failure if there is a type mismatch. That is just + in case some other code in this file (e.g., the WatchdogListener) needs + to process the parameter change. This is because rclpy runs all parameter + callbacks in sequence until one returns failure. + """ + num_updated_params = 0 + for param in params: + if "tree_kwargs" in param.name: + names = param.name.split(".") + if names[0] != "current" or len(names) != 4: + self.get_logger().warn(f"Parameter {param.name} cannot be changed") + continue + full_name = ".".join(names[1:]) + if full_name not in self.default_parameters: + self.get_logger().warn(f"Unknown parameter {param.name}") + continue + if isinstance(param.value, type(self.default_parameters[full_name])): + self.get_logger().warn( + f"Parameter {param.name} must be of type " + f"{type(self.default_parameters[full_name])} " + f"but is of type {type(param.value)}" + ) + return SetParametersResult(successful=False, reason="type mismatch") + if isinstance(param.value, collections.abc.Sequence): + self.overridden_parameters[full_name] = list(param.value) + else: + self.overridden_parameters[full_name] = param.value + num_updated_params += 1 + # Re-initialize the tree + server_name = names[1] + for action_server_params in self.action_server_params: + if action_server_params.server_name == server_name: + action_server_params.tree_kwargs[names[3]] = param.value + # pylint: disable=too-many-function-args + self.create_tree( + server_name, + action_server_params.tree_class, + action_server_params.tree_kwargs, + ) + break + else: + self.get_logger().warn(f"Parameter {param.name} cannot be changed") + continue + if num_updated_params > 0: + self.save_overridden_parameters() + return SetParametersResult(successful=True) + + def save_overridden_parameters(self) -> None: + """ + Overrides `ada_feeding_action_servers_current.yaml` with the current + values of `self.overridden_parameters`. + """ + # Convert the parameters to a dictionary of the right form + params = {} + params["overridden_parameters"] = list(self.overridden_parameters.keys()) + for full_name, value in self.overridden_parameters.items(): + params[full_name] = value + data = {"ada_feeding_action_servers": {"ros__parameters": {"current": params}}} + + # Write to yaml + package_path = get_package_share_directory("ada_feeding") + file_path = os.path.join( + package_path, "config", "ada_feeding_action_servers_current.yaml" + ) + self.get_logger().debug(f"Writing to {file_path} with data {data}") + with open(file_path, "w", encoding="utf-8") as file: + file.write("# This file is auto-generated by create_action_servers.py\n") + yaml.dump(data, file) + def create_action_servers( self, action_server_params: List[ActionServerParams] ) -> None: @@ -237,7 +347,8 @@ def create_action_servers( self._action_servers = [] self._action_types = {} self._tree_classes = {} - self._trees = [] + self._trees = {} + self._tree_action_servers = {} for params in action_server_params: self.get_logger().info( f"Creating action server {params.server_name} with type {params.action_type}" @@ -311,6 +422,29 @@ def cancel_callback(self, _: ServerGoalHandle) -> CancelResponse: self.get_logger().info("Received cancel request, accepting") return CancelResponse.ACCEPT + def create_tree( + self, + server_name: str, + tree_class: str, + tree_kwargs: Dict, + ) -> None: + """ + Creates the tree_action_server and tree objects for the given action server. + + Parameters + ---------- + server_name: The name of the action server. + tree_class: The class of the behavior tree, e.g., ada_feeding.trees.MoveToConfigurationTree. + tree_kwargs: The keyword arguments to pass to the behavior tree class. + """ + # Initialize the ActionServerBT object once + tree_action_server = self._tree_classes[tree_class](self, **tree_kwargs) + self._tree_action_servers[server_name] = tree_action_server + # Create and setup the tree once + tree = tree_action_server.create_tree(server_name) + self.setup_tree(tree) + self._trees[server_name] = tree + def setup_tree(self, tree: py_trees.trees.BehaviourTree) -> None: """ Runs the initial setup on a behavior tree after creating it. @@ -363,12 +497,7 @@ def get_execute_callback( ------- execute_callback: The callback function for the action server. """ - # Initialize the ActionServerBT object once - tree_action_server = self._tree_classes[tree_class](self, **tree_kwargs) - # Create and setup the tree once - tree = tree_action_server.create_tree(server_name) - self.setup_tree(tree) - self._trees.append(tree) + self.create_tree(server_name, tree_class, tree_kwargs) async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: """ @@ -388,6 +517,10 @@ async def execute_callback(goal_handle: ServerGoalHandle) -> Awaitable: # pylint: disable=broad-exception-caught # All exceptions need printing at shutdown try: + # Get the tree and action server + tree = self._trees[server_name] + tree_action_server = self._tree_action_servers[server_name] + # Send the goal to the behavior tree tree_action_server.send_goal(tree, goal_handle.request) @@ -473,7 +606,7 @@ def shutdown(self) -> None: Shutdown the node. """ self.get_logger().info("Shutting down CreateActionServers") - for tree in self._trees: + for _, tree in self._trees.items(): # Shutdown the tree tree.shutdown() From dd13335e069cc734677ef1bfe5e3f2aebfdb90f5 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 22 Dec 2023 18:05:55 -0800 Subject: [PATCH 2/7] Don't restrict parameter updates to tree_kwargs --- ada_feeding/scripts/create_action_servers.py | 45 +++++++++----------- 1 file changed, 21 insertions(+), 24 deletions(-) diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index e2faca31..3cb3f2fd 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -270,28 +270,28 @@ def parameter_callback(self, params: List[Parameter]) -> SetParametersResult: """ num_updated_params = 0 for param in params: + names = param.name.split(".") + if len(names) < 1 or names[0] != "current": + self.get_logger().warn(f"Parameter {param.name} cannot be changed") + continue + full_name = ".".join(names[1:]) + if full_name not in self.default_parameters: + self.get_logger().warn(f"Unknown parameter {param.name}") + continue + if isinstance(param.value, type(self.default_parameters[full_name])): + self.get_logger().warn( + f"Parameter {param.name} must be of type " + f"{type(self.default_parameters[full_name])} " + f"but is of type {type(param.value)}" + ) + return SetParametersResult(successful=False, reason="type mismatch") + if isinstance(param.value, collections.abc.Sequence): + self.overridden_parameters[full_name] = list(param.value) + else: + self.overridden_parameters[full_name] = param.value + num_updated_params += 1 + # If a tree_kwarg was set, re-initialize the tree if "tree_kwargs" in param.name: - names = param.name.split(".") - if names[0] != "current" or len(names) != 4: - self.get_logger().warn(f"Parameter {param.name} cannot be changed") - continue - full_name = ".".join(names[1:]) - if full_name not in self.default_parameters: - self.get_logger().warn(f"Unknown parameter {param.name}") - continue - if isinstance(param.value, type(self.default_parameters[full_name])): - self.get_logger().warn( - f"Parameter {param.name} must be of type " - f"{type(self.default_parameters[full_name])} " - f"but is of type {type(param.value)}" - ) - return SetParametersResult(successful=False, reason="type mismatch") - if isinstance(param.value, collections.abc.Sequence): - self.overridden_parameters[full_name] = list(param.value) - else: - self.overridden_parameters[full_name] = param.value - num_updated_params += 1 - # Re-initialize the tree server_name = names[1] for action_server_params in self.action_server_params: if action_server_params.server_name == server_name: @@ -303,9 +303,6 @@ def parameter_callback(self, params: List[Parameter]) -> SetParametersResult: action_server_params.tree_kwargs, ) break - else: - self.get_logger().warn(f"Parameter {param.name} cannot be changed") - continue if num_updated_params > 0: self.save_overridden_parameters() return SetParametersResult(successful=True) From 41b09e58caa7e50e3ff6adfaa2495f5f32d10fa7 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 22 Dec 2023 18:10:06 -0800 Subject: [PATCH 3/7] Change action_server_params to dict --- ada_feeding/scripts/create_action_servers.py | 46 ++++++++++---------- 1 file changed, 22 insertions(+), 24 deletions(-) diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index 3cb3f2fd..b3eed1b7 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -110,13 +110,13 @@ def __init__(self) -> None: # Create the action servers. self.create_action_servers(self.action_server_params) - def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: + def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParams]]: """ Read the parameters that specify what action servers to create. Returns ------- - action_server_params: A list of ActionServerParams objects. + action_server_params: A dict mapping server names to ActionServerParams objects. """ # Read the server names server_names = self.declare_parameter( @@ -150,7 +150,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: self.default_parameters = {} # Read each action server's params - action_server_params = [] + action_server_params = {} for server_name in server_names.value: # Get the action server's params action_type, tree_class, tick_rate = self.declare_parameters( @@ -246,14 +246,12 @@ def read_params(self) -> Tuple[Parameter, Parameter, List[ActionServerParams]]: ) continue - action_server_params.append( - ActionServerParams( - server_name=server_name, - action_type=action_type.value, - tree_class=tree_class.value, - tree_kwargs=tree_kwargs, - tick_rate=tick_rate.value, - ) + action_server_params[server_name] = ActionServerParams( + server_name=server_name, + action_type=action_type.value, + tree_class=tree_class.value, + tree_kwargs=tree_kwargs, + tick_rate=tick_rate.value, ) return action_server_params @@ -293,16 +291,16 @@ def parameter_callback(self, params: List[Parameter]) -> SetParametersResult: # If a tree_kwarg was set, re-initialize the tree if "tree_kwargs" in param.name: server_name = names[1] - for action_server_params in self.action_server_params: - if action_server_params.server_name == server_name: - action_server_params.tree_kwargs[names[3]] = param.value - # pylint: disable=too-many-function-args - self.create_tree( - server_name, - action_server_params.tree_class, - action_server_params.tree_kwargs, - ) - break + if server_name in self.action_server_params: + action_server_params = self.action_server_params[server_name] + kw = names[3] + action_server_params.tree_kwargs[kw] = param.value + # pylint: disable=too-many-function-args + self.create_tree( + server_name, + action_server_params.tree_class, + action_server_params.tree_kwargs, + ) if num_updated_params > 0: self.save_overridden_parameters() return SetParametersResult(successful=True) @@ -330,14 +328,14 @@ def save_overridden_parameters(self) -> None: yaml.dump(data, file) def create_action_servers( - self, action_server_params: List[ActionServerParams] + self, action_server_params: Dict[str, ActionServerParams] ) -> None: """ Create the action servers specified in the configuration file. Parameters ---------- - action_server_params: A list of ActionServerParams objects. + action_server_params: A dict mapping server names to ActionServerParams objects. """ # For every action server specified in the configuration file, create # an action server. @@ -346,7 +344,7 @@ def create_action_servers( self._tree_classes = {} self._trees = {} self._tree_action_servers = {} - for params in action_server_params: + for params in action_server_params.values(): self.get_logger().info( f"Creating action server {params.server_name} with type {params.action_type}" ) From bd1f4e124a06c60168bb81037767802e91124271 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 22 Dec 2023 18:20:09 -0800 Subject: [PATCH 4/7] Remove WatchdogLsitener params from default --- ada_feeding/config/ada_feeding_action_servers_default.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_default.yaml b/ada_feeding/config/ada_feeding_action_servers_default.yaml index 6e433881..b013cb17 100644 --- a/ada_feeding/config/ada_feeding_action_servers_default.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_default.yaml @@ -1,10 +1,10 @@ # NOTE: You have to change this node name if you change the node name in the launchfile. ada_feeding_action_servers: ros__parameters: - default: - # If we haven't received a message from the watchdog in this time, stop all robot motions - watchdog_timeout_sec: 0.5 # sec + # If we haven't received a message from the watchdog in this time, stop all robot motions + watchdog_timeout_sec: 0.5 # sec + default: # A list of the names of the action servers to create. Each one must have # it's own parameters (below) specifying action_type and tree_class. server_names: From 6b625b45e33ba04088a9bcd72790b33eebe5de31 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 22 Dec 2023 19:08:59 -0800 Subject: [PATCH 5/7] Make sequences stored as lists by convention --- .../ada_feeding_action_servers_default.yaml | 3 +- ada_feeding/scripts/create_action_servers.py | 31 ++++++++++++------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/ada_feeding/config/ada_feeding_action_servers_default.yaml b/ada_feeding/config/ada_feeding_action_servers_default.yaml index b013cb17..b9594b0c 100644 --- a/ada_feeding/config/ada_feeding_action_servers_default.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_default.yaml @@ -81,7 +81,7 @@ ada_feeding_action_servers: max_velocity_scaling_factor: 0.65 # optional in (0.0, 1.0], default: 0.1 tick_rate: 10 # Hz, optional, default: 30 - # Parameters for the MoveToMouth action + # Parameters for the MoveToStagingConfiguration action MoveToStagingConfiguration: # required action_type: ada_feeding_msgs.action.MoveTo # required tree_class: ada_feeding.trees.MoveToConfigurationWithWheelchairWallTree # required @@ -136,7 +136,6 @@ ada_feeding_action_servers: - face_detection_timeout - fork_target_orientation_from_mouth - plan_distance_from_mouth - tree_kwargs: # optional face_detection_msg_timeout: 10.0 # sec face_detection_timeout: 5.0 # sec diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index b3eed1b7..f9530ea0 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -222,7 +222,11 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam read_only=True, ), ) - self.default_parameters[full_name] = default_value.value + if isinstance(default_value, collections.abc.Sequence): + default_value = list(default_value.value) + else: + default_value = default_value.value + self.default_parameters[full_name] = default_value current_value = self.declare_parameter( f"current.{full_name}", descriptor=ParameterDescriptor( @@ -231,11 +235,15 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam dynamic_typing=True, ), ) + if isinstance(current_value, collections.abc.Sequence): + current_value = list(current_value.value) + else: + current_value = current_value.value if full_name in overridden_parameters: - tree_kwargs[kw] = current_value.value - self.overridden_parameters[full_name] = current_value.value + tree_kwargs[kw] = current_value + self.overridden_parameters[full_name] = current_value else: - tree_kwargs[kw] = default_value.value + tree_kwargs[kw] = default_value else: tree_kwargs = {} @@ -276,17 +284,18 @@ def parameter_callback(self, params: List[Parameter]) -> SetParametersResult: if full_name not in self.default_parameters: self.get_logger().warn(f"Unknown parameter {param.name}") continue - if isinstance(param.value, type(self.default_parameters[full_name])): + if isinstance(param.value, collections.abc.Sequence): + param_value = list(param.value) + else: + param_value = param.value + if not isinstance(param_value, type(self.default_parameters[full_name])): self.get_logger().warn( f"Parameter {param.name} must be of type " f"{type(self.default_parameters[full_name])} " - f"but is of type {type(param.value)}" + f"but is of type {type(param_value)}" ) return SetParametersResult(successful=False, reason="type mismatch") - if isinstance(param.value, collections.abc.Sequence): - self.overridden_parameters[full_name] = list(param.value) - else: - self.overridden_parameters[full_name] = param.value + self.overridden_parameters[full_name] = param_value num_updated_params += 1 # If a tree_kwarg was set, re-initialize the tree if "tree_kwargs" in param.name: @@ -294,7 +303,7 @@ def parameter_callback(self, params: List[Parameter]) -> SetParametersResult: if server_name in self.action_server_params: action_server_params = self.action_server_params[server_name] kw = names[3] - action_server_params.tree_kwargs[kw] = param.value + action_server_params.tree_kwargs[kw] = param_value # pylint: disable=too-many-function-args self.create_tree( server_name, From d9f4af005cbd7689d7b1aad66172ce10556bf93a Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 23 Dec 2023 09:05:09 -0800 Subject: [PATCH 6/7] Addressed PR comment --- ada_feeding/ada_feeding/trees/move_to_mouth_tree.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py index fab0556e..1560e79b 100644 --- a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py @@ -9,8 +9,9 @@ # that they have similar code. # Standard imports +from collections.abc import Sequence import os -from typing import List, Tuple +from typing import Annotated, Tuple # Third-party imports from ament_index_python.packages import get_package_share_directory @@ -76,6 +77,8 @@ class MoveToMouthTree(MoveToTree): # pylint: disable=too-many-instance-attributes, too-many-arguments # Bite transfer is a big part of the robot's behavior, so it makes # sense that it has lots of attributes/arguments. + # pylint: disable=dangerous-default-value + # plan_distance_from_mouth must be a list because ROS params only supports lists. def __init__( self, @@ -90,7 +93,7 @@ def __init__( allowed_face_distance: Tuple[float, float] = (0.4, 1.25), face_detection_msg_timeout: float = 5.0, face_detection_timeout: float = 2.5, - plan_distance_from_mouth: List[float] = [0.025, 0.0, -0.01], + plan_distance_from_mouth: Annotated[Sequence[float], 3] = [0.025, 0.0, -0.01], fork_target_orientation_from_mouth: Tuple[float, float, float, float] = ( 0.5, -0.5, From 58f1fd816f5d000d8663e1f7f2032ba41163e872 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Sat, 23 Dec 2023 09:10:54 -0800 Subject: [PATCH 7/7] Addressed PR comments --- ada_feeding/scripts/create_action_servers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index f9530ea0..c85e3b5a 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -610,7 +610,7 @@ def shutdown(self) -> None: Shutdown the node. """ self.get_logger().info("Shutting down CreateActionServers") - for _, tree in self._trees.items(): + for tree in self._trees.values(): # Shutdown the tree tree.shutdown()