From 11ddeee0c8260db4469f4ca0f2f2d44e8de406d3 Mon Sep 17 00:00:00 2001
From: lukeschmitt-tr <85308904+lukeschmitt-tr@users.noreply.github.com>
Date: Mon, 26 Feb 2024 09:45:13 -0600
Subject: [PATCH] [xs_modules] More verbose errors (#63)

* [xs_modules] Add more descriptive errors

* Fix linting issue
---
 .../interbotix_xs_modules/xs_robot/arm.py          | 14 +++++++++++---
 .../interbotix_xs_modules/xs_robot/core.py         |  6 ++----
 2 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py
index 8f643ab..f311050 100644
--- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py
+++ b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py
@@ -34,6 +34,7 @@
 
 import math
 from threading import Thread
+import sys
 import time
 from typing import Any, List, Tuple, Union
 
@@ -178,7 +179,7 @@ def shutdown(self) -> None:
             time.sleep(0.5)
         else:
             self.core.get_logger().error(
-                "Cannot perform shutdown due to lack of ownership"
+                'Cannot perform shutdown due to lack of ownership'
             )
 
 
@@ -227,16 +228,23 @@ def __init__(
             rclpy.spin_once(self.core)
 
         self.group_info: RobotInfo.Response = self.future_group_info.result()
+        if self.group_info.num_joints != self.robot_des.Slist.shape[1]:
+            self.core.get_logger().fatal(
+                f"Number of joints in group '{group_name}' does not match Modern Robotics "
+                f"description for arm model '{robot_model}' ({self.group_info.num_joints} != "
+                f'{self.robot_des.Slist.shape[1]}).'
+            )
+            sys.exit(1)
         if self.group_info.profile_type != 'time':
             self.core.get_logger().error(
                 "Please set the group's 'profile_type' to 'time'."
             )
-            exit(1)
+            sys.exit(1)
         if self.group_info.mode != 'position':
             self.core.get_logger().error(
                 "Please set the group's 'operating mode' to 'position'."
             )
-            exit(1)
+            sys.exit(1)
 
         # initialize initial IK guesses
         self.initial_guesses = [[0.0] * self.group_info.num_joints for _ in range(3)]
diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/core.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/core.py
index c46e957..f352e93 100644
--- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/core.py
+++ b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/core.py
@@ -29,7 +29,6 @@
 """Contains the `InterbotixRobotXSCore` class that interfaces with the interbotix_xs_sdk."""
 
 import copy
-import sys
 from threading import Lock
 from typing import Dict, List
 
@@ -130,14 +129,13 @@ def __init__(
         )
 
         # Check for xs_sdk by looking for set_operating_modes
-        if not self.srv_set_op_modes.wait_for_service(timeout_sec=10.0):
+        while not self.srv_set_op_modes.wait_for_service(timeout_sec=5.0) and rclpy.ok():
             self.get_logger().error(
                 (
                     f"Failed to find services under namespace '{self.robot_name}'. Is the xs_sdk "
-                    'running? Shutting down...'
+                    'running under that namespace?'
                 )
             )
-            sys.exit(1)
         self.srv_set_pids.wait_for_service()
         self.srv_set_reg.wait_for_service()
         self.srv_get_reg.wait_for_service()