Allow parameter changes while keeping defaults

[amalnanavati]

Description

This PR allows us to change non-read-only parameters for create_action_server, have them persist across runs of ada_feeding_launch.xml, but still maintain access to the original parameters (e.g., in case we have to revert them). In practice, only the tree_kwargs are non-read-only.

It achieves this through the following:

1. Parameters for create_action_server are not defined in two files: (a) ada_feeding_action_servers_default.yaml and (b) ada_feeding_action_servers_current.yaml.
   1. All parameters but the ones for the WatchdogListener are placed under the default namespace in ada_feeding_action_servers_default.yaml.
   2. ada_feeding_action_servers_current.yaml contains parameters in the current namespace: (a) current.overridden_parameters which is a list of the parameters that are overridden, and (b) the values for each of those parameters.
2. On the node side, all parameters in the default' namespace are declared as read-only. However, for the ones that the node wants to make over-writeable, it declares a read-write version of that parameter in thecurrent` namespace.
3. When the node receives a set parameter request for a parameter in the current namespace, it does the following:
   1. Checks (e.g., that the parameter type is correct)
   2. Update the parameter. In particularly, since the only non-read-only parameters are tree_kwargs, it re-creates the tree with the new kwargs.
   3. Write the updated overridden parameters to ada_feeding_action_servers_current.yaml in the share folder.

Testing procedure

This was tested in sim.

• [x] Setup
  • [x] ros2 run ada_feeding dummy_ft_sensor.py
  • [x] ros2 launch feeding_web_app_ros2_test feeding_web_app_dummy_nodes_launch.xml run_web_bridge:=false run_motion:=false
  • [x] (Update path based on your current directory) ros2 run ada_feeding_perception republisher --ros-args --params-file src/ada_feeding/ada_feeding_perception/config/republisher.yaml
  • [x] ros2 launch ada_feeding ada_feeding_launch.xml use_estop:=false
• [x] Testing without MoveIt
  • [x] Run ros2 param list. For the /ada_feeding_action_servers node, verify that all the tree_kwargs in default are also in current.
  • [x] Get the value of a param in current and verify it is unset: e.g., ros2 service call /ada_feeding_action_servers/get_parameters rcl_interfaces/srv/GetParameters "{names: ['current.MoveAbovePlate.tree_kwargs.f_mag']}"
  • [x] Get the value of the corresponding param in default and verify that it is set as expected (should be 4.0): e.g., ros2 service call /ada_feeding_action_servers/get_parameters rcl_interfaces/srv/GetParameters "{names: ['default.MoveAbovePlate.tree_kwargs.f_mag']}"
  • [x] Attempt to change the default parameter, and verify that it fails: e.g., ros2 service call /ada_feeding_action_servers/set_parameters rcl_interfaces/srv/SetParameters "{parameters: [{name: 'default.MoveAbovePlate.tree_kwargs.f_mag', value: {type: 3, double_value: 1.0}}]}"
  • [x] Attempt to change the current parameter to an incorrect type, and verify that it failes: e.g., ros2 service call /ada_feeding_action_servers/set_parameters rcl_interfaces/srv/SetParameters "{parameters: [{name: 'current.MoveAbovePlate.tree_kwargs.f_mag', value: {type: 4, string_value: 'ASDFGHJKL'}}]}"
    • [x] Verify that in the share directory (within the install folder of the workspace), ada_feeding_action_servers_current.yaml is unchanged.
  • [x] Attempt to change the current parameter to a correct type, and verify that it works: e.g., ros2 service call /ada_feeding_action_servers/set_parameters rcl_interfaces/srv/SetParameters "{parameters: [{name: 'current.MoveAbovePlate.tree_kwargs.f_mag', value: {type: 3, double_value: 1.0}}]}"
    • [x] Verify that in the share directory (within the install folder of the workspace), ada_feeding_action_servers_current.yaml has changed to reflect the overridden parameter.
    • [x] Verify that in the output of the node, you see some info logs indicating that the MoveAbovePlate tree was re-created.
  • [x] Get the value of the param in default and verify that it is unchanged (should be 4.0): e.g., ros2 service call /ada_feeding_action_servers/get_parameters rcl_interfaces/srv/GetParameters "{names: ['default.MoveAbovePlate.tree_kwargs.f_mag']}"
  • [x] Get the current value of the param and verify it is set correctly (should be 1.0): e.g., ros2 service call /ada_feeding_action_servers/get_parameters rcl_interfaces/srv/GetParameters "{names: ['current.MoveAbovePlate.tree_kwargs.f_mag']}"
• [x] Testing with MoveIt
  • [x] Launch MoveIt: ros2 launch ada_moveit demo.launch.py sim:=mock
  • [x] Move the robot to the user's mouth, and verify that it stops 2.5cm away from the mouth.
    • [x] ros2 action send_goal /MoveToStagingConfiguration ada_feeding_msgs/action/MoveTo "{}" --feedback
    • [x] ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveToMouth "{}" --feedback
  • [x] Move back to the staging configuration: ros2 action send_goal /MoveFromMouthToStagingConfiguration ada_feeding_msgs/action/MoveTo "{}" --feedback
  • [x] Change the distance to mouth to be 10cm, and verify that it succeeds: ros2 service call /ada_feeding_action_servers/set_parameters rcl_interfaces/srv/SetParameters "{parameters: [{name: 'current.MoveToMouth.tree_kwargs.plan_distance_from_mouth', value: {type: 8, double_array_value: [0.10, 0.0, -0.01]}}]}"
  • [x] Move to the mouth, and verify that it stops 10cm away: ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveToMouth "{}" --feedback
  • [x] Verify that the parameter change persists across runs of the code:
    • [x] Terminate the ada_feeding_launch.xml code and the ada_moveit code.
    • [x] Re-run both
    • [x] Move the robot to the user's mouth, and verify that it stops 10cm away from the mouth.
      • [x] ros2 action send_goal /MoveToStagingConfiguration ada_feeding_msgs/action/MoveTo "{}" --feedback
      • [x] ros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveToMouth "{}" --feedback

Before opening a pull request

• [x] Format your code using black formatter python3 -m black .
• [x] Run your code through pylint and address all warnings/errors. The only warnings that are acceptable to not address is TODOs that should be addressed in a future PR. From the top-level ada_feeding directory, run: pylint --recursive=y --rcfile=.pylintrc ..

Before Merging

• [ ] Squash & Merge