From 9f1fa8b0f2bf4d4c7757ee71fe184081361304fb Mon Sep 17 00:00:00 2001
From: Isaac Acevedo <isaac.acevedo@pal-robotics.com>
Date: Tue, 5 Nov 2024 17:29:18 +0100
Subject: [PATCH] Fix excessive cpu consumption

---
 joy_teleop/joy_teleop/incrementer_server.py | 38 ++++++---------------
 1 file changed, 10 insertions(+), 28 deletions(-)

diff --git a/joy_teleop/joy_teleop/incrementer_server.py b/joy_teleop/joy_teleop/incrementer_server.py
index c8ed0b8..f19b2d8 100644
--- a/joy_teleop/joy_teleop/incrementer_server.py
+++ b/joy_teleop/joy_teleop/incrementer_server.py
@@ -40,14 +40,12 @@
 #   * Jeremie Deray (artivis)
 #   * Borong Yuan
 
-import time
 
 from control_msgs.msg import JointTrajectoryControllerState as JTCS
 import rclpy
+from rclpy.wait_for_message import wait_for_message
 from rclpy.action import ActionServer
-from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.duration import Duration
-from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from teleop_tools_msgs.action import Increment as TTIA
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
@@ -58,42 +56,28 @@ class IncrementerServer(Node):
     def __init__(self):
         super().__init__('incrementer_server', namespace='joint_trajectory_controller')
 
-        cb_group = ReentrantCallbackGroup()
-
-        self._has_new_message = False
-
         self._as = ActionServer(self, TTIA, 'increment',
-                                self._as_cb, callback_group=cb_group)
+                                self._as_cb)
 
         self._command_pub = self.create_publisher(
             JointTrajectory, 'joint_trajectory', 1)
 
-        self._state_sub = self.create_subscription(
-            JTCS, 'controller_state', self._state_cb, 1, callback_group=cb_group)
-
         self._goal = JointTrajectory()
-        self.get_logger().info('Connected to {}'.format(self.get_namespace()))
+        self.get_logger().info(f'Connected to {self.get_namespace()}')
 
     def _as_cb(self, goal):
         self.increment_by(goal.request.increment_by)
         goal.succeed()
         return TTIA.Result()
 
-    def _state_cb(self, state):
-        self._state = state
-        self._has_new_message = True
-
-    # TODO(artivis) change after
-    # https://github.com/ros2/rclcpp/issues/520
-    # has landed
-    def _wait_for_new_message(self):
-        self._has_new_message = False
-        while not self._has_new_message:
-            time.sleep(0.01)
-        return self._state
+    def _wait_for_state_message(self):
+        msg_ok = False
+        while not msg_ok:
+            msg_ok, state = wait_for_message(JTCS, self, 'controller_state')
+        return state
 
     def increment_by(self, increment):
-        state = self._wait_for_new_message()
+        state = self._wait_for_state_message()
         self._goal.joint_names = state.joint_names
         self._value = state.feedback.positions
         self._value = [x + y for x, y in zip(self._value, increment)]
@@ -109,11 +93,9 @@ def main():
     rclpy.init()
 
     node = IncrementerServer()
-    executor = MultiThreadedExecutor()
-    executor.add_node(node)
 
     try:
-        executor.spin()
+        rclpy.spin(node)
     except KeyboardInterrupt:
         pass