How to use ActionClient with mqtt

asked 2021-10-27 18:21:11 -0500

simona gravatar image

Hi, I'm trying to read action message from mqtt queue and use send_goal method as a consumer's callback. But the problem is that then send_goal_async method never gets called and the goal is never executed

FIBONACCI_QUEUE = 'fibonacci_queue'

def get_channel():
    connection = pika.BlockingConnection(pika.ConnectionParameters('localhost'))

    channel = connection.channel()
    channel.queue_declare(queue=FIBONACCI_QUEUE, durable=True)

    return channel
class FibonacciActionClient(Node):

    def __init__(self):
        super().__init__('fibonacci_action_client')

        self._action_client = ActionClient(self, Fibonacci, 'fibonacci')

        self.channel = get_channel()
        self.channel.basic_consume(
           queue=FIBONACCI_QUEUE, on_message_callback=self.send_goal, auto_ack=True)
        self.channel.start_consuming()

    def send_goal(self, ch, method, properties, body):
        data = json.loads(body)
        order = data['order']

        goal_msg = Fibonacci.Goal()
        goal_msg.order = order

        self._action_client.wait_for_server()

        self._send_goal_future = self._action_client.send_goal_async(goal_msg)

        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
        return

        self.get_logger().info('Goal accepted :)')

        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
       result = future.result().result
       self.get_logger().info('Result: {0}'.format(result.sequence))

def main(args=None):
    rclpy.init(args=args)
    node = FibonacciActionClient()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
edit retag flag offensive close merge delete