From b618d166a69e716d987e73fba4f3730458fe4aff Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Fri, 8 Dec 2023 13:42:43 +0100
Subject: [PATCH] Fix rqt jtc bugs for continuous joints and other minor bugs
 (#890)

(cherry picked from commit e8a127f2b8bc6b5561f54d4afcb30a641f60ec3a)
---
 .../joint_limits_urdf.py                      | 57 ++++++++++++-------
 .../joint_trajectory_controller.py            | 10 +++-
 2 files changed, 42 insertions(+), 25 deletions(-)

diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py
index 7914a76b17..5655e12f7a 100644
--- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py
+++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py
@@ -14,6 +14,10 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+# Code inspired on the joint_state_publisher package by David Lu!!!
+# https://github.com/ros/robot_model/blob/indigo-devel/
+# joint_state_publisher/joint_state_publisher/joint_state_publisher
+
 # TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got
 #  Exception: Required attribute not set in XML: upper
 # upper is an optional attribute, so I don't understand what's going on
@@ -33,21 +37,24 @@ def callback(msg):
     description = msg.data
 
 
-def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=True):
-    global description
-    use_small = use_smallest_joint_limits
-    use_mimic = True
-
-    # Code inspired on the joint_state_publisher package by David Lu!!!
-    # https://github.com/ros/robot_model/blob/indigo-devel/
-    # joint_state_publisher/joint_state_publisher/joint_state_publisher
-
+def subscribe_to_robot_description(node, key="robot_description"):
     qos_profile = rclpy.qos.QoSProfile(depth=1)
     qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL
     qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE
 
-    node.create_subscription(String, "/robot_description", callback, qos_profile)
-    rclpy.spin_once(node)
+    node.create_subscription(String, key, callback, qos_profile)
+
+
+def get_joint_limits(node, use_smallest_joint_limits=True):
+    global description
+    use_small = use_smallest_joint_limits
+    use_mimic = True
+
+    count = 0
+    while description == "" and count < 10:
+        print("Waiting for the robot_description!")
+        count += 1
+        rclpy.spin_once(node, timeout_sec=1.0)
 
     free_joints = {}
     dependent_joints = {}
@@ -66,21 +73,27 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr
                 name = child.getAttribute("name")
                 try:
                     limit = child.getElementsByTagName("limit")[0]
-                except IndexError:
-                    continue
-                if jtype == "continuous":
-                    minval = -pi
-                    maxval = pi
-                else:
                     try:
                         minval = float(limit.getAttribute("lower"))
                         maxval = float(limit.getAttribute("upper"))
                     except ValueError:
-                        continue
-                try:
-                    maxvel = float(limit.getAttribute("velocity"))
-                except ValueError:
-                    continue
+                        if jtype == "continuous":
+                            minval = -pi
+                            maxval = pi
+                        else:
+                            raise Exception(
+                                f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!"
+                            )
+                    try:
+                        maxvel = float(limit.getAttribute("velocity"))
+                    except ValueError:
+                        raise Exception(
+                            f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!"
+                        )
+                except IndexError:
+                    raise Exception(
+                        f"Missing limits tag for the joint : {name} in the robot_description!"
+                    )
                 safety_tags = child.getElementsByTagName("safety_controller")
                 if use_small and len(safety_tags) == 1:
                     tag = safety_tags[0]
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 99f43e125e..162977cdfe 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
@@ -29,7 +29,7 @@
 
 from .utils import ControllerLister, ControllerManagerLister
 from .double_editor import DoubleEditor
-from .joint_limits_urdf import get_joint_limits
+from .joint_limits_urdf import get_joint_limits, subscribe_to_robot_description
 from .update_combo import update_combo
 
 # TODO:
@@ -170,6 +170,9 @@ def __init__(self, context):
         self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
         self._update_jtc_list_timer.start()
 
+        # subscriptions
+        subscribe_to_robot_description(self._node)
+
         # Signal connections
         w = self._widget
         w.enable_button.toggled.connect(self._on_jtc_enabled)
@@ -460,8 +463,9 @@ def _jtc_joint_names(jtc_info):
 
     joint_names = []
     for interface in jtc_info.claimed_interfaces:
-        name = interface.split("/")[0]
-        joint_names.append(name)
+        name = interface.split("/")[-2]
+        if name not in joint_names:
+            joint_names.append(name)
 
     return joint_names