From 9f1fa8b0f2bf4d4c7757ee71fe184081361304fb Mon Sep 17 00:00:00 2001 From: Isaac Acevedo Date: Tue, 5 Nov 2024 17:29:18 +0100 Subject: [PATCH 1/2] 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 From ff8a3f1a3f14fefeffd9b6046b62234aea0de735 Mon Sep 17 00:00:00 2001 From: Isaac Acevedo Date: Wed, 6 Nov 2024 08:55:14 +0100 Subject: [PATCH 2/2] Reorder imports --- joy_teleop/joy_teleop/incrementer_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joy_teleop/joy_teleop/incrementer_server.py b/joy_teleop/joy_teleop/incrementer_server.py index f19b2d8..d4e5087 100644 --- a/joy_teleop/joy_teleop/incrementer_server.py +++ b/joy_teleop/joy_teleop/incrementer_server.py @@ -43,10 +43,10 @@ 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.duration import Duration from rclpy.node import Node +from rclpy.wait_for_message import wait_for_message from teleop_tools_msgs.action import Increment as TTIA from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint