MoveAbovePlate before AcquireFood (#89)

* MoveAbovePlate before AcquireFood

* Black format

* Remove auto-Shutdown, this actually makes it harder to see the result, need manual Ctrl-C after each test

Author: egordon

feeding_web_app_ros2_test/feeding_web_app_ros2_test/AcquireFoodClient.py

@@ -9,22 +9,26 @@
 from rclpy.node import Node
 
 # Local imports
-from ada_feeding_msgs.action import AcquireFood
+from ada_feeding_msgs.action import AcquireFood, MoveTo
 
 
 class AcquireFoodClient(Node):
     def __init__(self):
         super().__init__("acquire_food_client")
+        self._above_client = ActionClient(self, MoveTo, "/MoveAbovePlate")
         self._action_client = ActionClient(self, AcquireFood, "/AcquireFood")
 
         self.declare_parameter("request_path", rclpy.Parameter.Type.STRING)
         request_path = self.get_parameter("request_path")
         with open(request_path.value, "rb") as file:
             self.goal_msg = pickle.load(file)
 
-    def send_goal(self):
-        self._action_client.wait_for_server()
+    def move_above_plate(self):
+        self._above_client.wait_for_server()
+        return self._above_client.send_goal_async(MoveTo.Goal())
 
+    def acquire_food(self):
+        self._action_client.wait_for_server()
         return self._action_client.send_goal_async(self.goal_msg)
 
 
@@ -33,8 +37,28 @@ def main(args=None):
 
     action_client = AcquireFoodClient()
 
+    # First Move Above Plate
+    # Send Goal
+    future = action_client.move_above_plate()
+    rclpy.spin_until_future_complete(action_client, future)
+
+    # Check Accept
+    goal_handle = future.result()
+    if not goal_handle.accepted:
+        action_client.get_logger().error("MoveAbovePlate Rejected")
+        return
+    action_client.get_logger().info("MoveAbovePlate Accepted")
+
+    # Get Result
+    future = goal_handle.get_result_async()
+    rclpy.spin_until_future_complete(action_client, future)
+    result = future.result().result
+    if result.status != result.STATUS_SUCCESS:
+        action_client.get_logger().error("MoveAbovePlate Failed")
+        return
+
     # Send Goal
-    future = action_client.send_goal()
+    future = action_client.acquire_food()
     rclpy.spin_until_future_complete(action_client, future)
 
     # Check Accept

feeding_web_app_ros2_test/launch/feeding_dummy_acquirefood_launch.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from launch_ros.substitutions import FindPackageShare
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, Shutdown
+from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import AnyLaunchDescriptionSource
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import (
@@ -62,7 +62,6 @@ def generate_launch_description():
             parameters=[
                 request_path,
             ],
-            on_exit=Shutdown(),
         )
     )