From f00c8abd2b4f608810861a0caca186d633311991 Mon Sep 17 00:00:00 2001
From: gwalck <guillaume.walck@stoglrobotics.de>
Date: Sun, 4 Jun 2023 17:57:17 +0200
Subject: [PATCH] Adapted rqt_jtc to newest control_msgs for jtc (#643)

(cherry picked from commit b47b42df93e264c901ee8293b4db3c4c2e1fcc19)
---
 .../joint_trajectory_controller.py            | 26 +++++++++----------
 1 file changed, 13 insertions(+), 13 deletions(-)

diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
index 7e8ec4948e..99f43e125e 100644
--- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
+++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py
@@ -80,8 +80,8 @@ class JointTrajectoryController(Plugin):
     the following requisites:
         - The controller type contains the C{JointTrajectoryController}
         substring, e.g., C{position_controllers/JointTrajectoryController}
-        - The controller exposes the C{command} and C{state} topics in its
-        ROS interface.
+        - The controller exposes the C{command} and C{controller_state} topics
+        in its ROS interface.
 
     Additionally, there must be a URDF loaded with a valid joint limit
     specification, namely position (when applicable) and velocity limits.
@@ -252,10 +252,10 @@ def _update_jtc_list(self):
     def _on_speed_scaling_change(self, val):
         self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
 
-    def _on_joint_state_change(self, actual_pos):
-        assert len(actual_pos) == len(self._joint_pos)
-        for name in actual_pos.keys():
-            self._joint_pos[name]["position"] = actual_pos[name]
+    def _on_joint_state_change(self, current_pos):
+        assert len(current_pos) == len(self._joint_pos)
+        for name in current_pos.keys():
+            self._joint_pos[name]["position"] = current_pos[name]
 
     def _on_cm_change(self, cm_ns):
         self._cm_ns = cm_ns
@@ -289,11 +289,11 @@ def _on_jtc_enabled(self, val):
         self._speed_scaling_widget.setEnabled(val)
 
         if val:
-            # Widgets send desired position commands to controller
+            # Widgets send reference position commands to controller
             self._update_act_pos_timer.stop()
             self._update_cmd_timer.start()
         else:
-            # Controller updates widgets with actual position
+            # Controller updates widgets with feedback position
             self._update_cmd_timer.stop()
             self._update_act_pos_timer.start()
 
@@ -332,7 +332,7 @@ def _load_jtc(self):
 
         # Setup ROS interfaces
         jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
-        state_topic = jtc_ns + "/state"
+        state_topic = jtc_ns + "/controller_state"
         cmd_topic = jtc_ns + "/joint_trajectory"
         self._state_sub = self._node.create_subscription(
             JointTrajectoryControllerState, state_topic, self._state_cb, 1
@@ -404,12 +404,12 @@ def _unregister_executor(self):
             self._executor = None
 
     def _state_cb(self, msg):
-        actual_pos = {}
+        current_pos = {}
         for i in range(len(msg.joint_names)):
             joint_name = msg.joint_names[i]
-            joint_pos = msg.actual.positions[i]
-            actual_pos[joint_name] = joint_pos
-        self.jointStateChanged.emit(actual_pos)
+            joint_pos = msg.feedback.positions[i]
+            current_pos[joint_name] = joint_pos
+        self.jointStateChanged.emit(current_pos)
 
     def _update_single_cmd_cb(self, val, name):
         self._joint_pos[name]["command"] = val