[WIP] Debugging action_client

scripts/debug.yaml

@@ -0,0 +1,34 @@
+timeout: 240            # how long to wait for the host
+
+hosts:
+  localhost:                   # the hostname of the machine
+    os: linux           # the operating system of the host
+    user: $USER         # the user to be used
+    port: 22            # the port that is checked to determine if a service on the host already up
+                        # if port is not defined, the check is disabled
+    check_nfs: false    # wait for the nfs mount to be available on the localhost. If not defined
+                        # the check is disabled (only for linux os)
+
+sessions:
+  roscore:
+    host: localhost
+    user: $USER
+    command: "source_amr_ros1 && roscore"
+    prio: 0
+    locked: true
+    wait_for_core: false
+  server_ros2:
+    host: localhost
+    user: $USER
+    command: "source_amr_ros2 && cd ~/git/amr_ros2_interfaces_ws/src/ros1_bridge/scripts && ./ros2_action_server.py"
+    prio: 0
+  client_ros1:
+    host: localhost
+    user: $USER
+    command: "source_amr_ros1 && cd ~/git/amr_ros2_interfaces_ws/src/ros1_bridge/scripts && ./ros1_action_client.py"
+    prio: 0
+  ab_exec_tj:
+    host: localhost
+    user: $USER
+    command: "source_amr_ros_bridge && ros2 run ros1_bridge action_bridge ros2 action_tutorials_interfaces action/Fibonacci fibonacci"
+    prio: 2

scripts/ros1_action_client.py

@@ -0,0 +1,57 @@
+#! /usr/bin/env python3
+
+from __future__ import print_function
+
+import sys
+import random
+
+import rospy
+# Brings in the SimpleActionClient
+import actionlib
+
+# Brings in the messages used by the fibonacci action, including the
+# goal message and the result message.
+from action_tutorials_interfaces.msg import FibonacciAction, FibonacciGoal, FibonacciFeedback
+
+
+class FibClient:
+
+    def __init__(self):
+        # Creates the SimpleActionClient, passing the type of the action
+        # (FibonacciAction) to the constructor.
+        self._ac = actionlib.SimpleActionClient('fibonacci', FibonacciAction)
+
+        # Waits until the action server has started up and started
+        # listening for goals.
+        self._ac.wait_for_server()
+
+    def run(self):
+        # Creates a goal to send to the action server.
+        goal = FibonacciGoal(order=20)
+
+        # Sends the goal to the action server.
+        self._ac.send_goal(goal, feedback_cb=self.accept_feedback)
+
+        # Waits for the server to finish performing the action.
+        self._ac.wait_for_result()
+
+        # Prints out the result of executing the action
+        return self._ac.get_result()  # A FibonacciResult
+
+    def accept_feedback(self, feedback: FibonacciFeedback):
+        rospy.loginfo(f"Got feedback: {feedback}")
+        # if random.randint(0, 1):
+        #     rospy.loginfo("Randomly aborting")
+        #     self._ac.cancel_all_goals()
+
+if __name__ == '__main__':
+    try:
+        # Initializes a rospy node so that the SimpleActionClient can
+        # publish and subscribe over ROS.
+        rospy.init_node('fibonacci_client_py')
+        client = FibClient()
+        for i in range(1000):
+            result = client.run()
+            print(f"Result {i}:", ', '.join([str(n) for n in result.sequence]))
+    except rospy.ROSInterruptException:
+        print("program interrupted before completion", file=sys.stderr)

scripts/ros2_action_server.py

@@ -0,0 +1,53 @@
+#! /usr/bin/env python3
+
+import time
+
+import rclpy
+from rclpy.action import ActionServer
+from rclpy.node import Node
+
+from action_tutorials_interfaces.action import Fibonacci
+
+
+class FibonacciActionServer(Node):
+
+    def __init__(self):
+        super().__init__('fibonacci_action_server')
+        self._action_server = ActionServer(
+            self,
+            Fibonacci,
+            'fibonacci',
+            self.execute_callback)
+
+    def execute_callback(self, goal_handle):
+        self.get_logger().info('Executing goal...')
+
+        feedback_msg = Fibonacci.Feedback()
+        feedback_msg.partial_sequence = [0, 1]
+
+        for i in range(1, goal_handle.request.order):
+            feedback_msg.partial_sequence.append(
+                feedback_msg.partial_sequence[i] + feedback_msg.partial_sequence[i-1])
+            self.get_logger().info('Feedback: {0}'.format(feedback_msg.partial_sequence))
+            goal_handle.publish_feedback(feedback_msg)
+            time.sleep(0.1)
+            # goal_handle.abort()
+            # break
+            # time.sleep(1)
+        # goal_handle.succeed()
+
+        result = Fibonacci.Result()
+        result.sequence = feedback_msg.partial_sequence
+        return result
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    fibonacci_action_server = FibonacciActionServer()
+
+    rclpy.spin(fibonacci_action_server)
+
+
+if __name__ == '__main__':
+    main()