From 1f474aa3d82599497773ab269046fb83af820360 Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Thu, 11 Jun 2020 03:57:52 -0400
Subject: [PATCH] Cleaned up imports

---
 stretch_core/nodes/keyboard_teleop | 36 +++++-------------------
 stretch_core/nodes/stretch_driver  | 44 ++++++++++--------------------
 2 files changed, 22 insertions(+), 58 deletions(-)

diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop
index c81db72..767025a 100755
--- a/stretch_core/nodes/keyboard_teleop
+++ b/stretch_core/nodes/keyboard_teleop
@@ -1,33 +1,12 @@
 #!/usr/bin/env python
 
-from __future__ import print_function
-
+import math
 import keyboard as kb
-
-from sensor_msgs.msg import JointState
-from geometry_msgs.msg import Twist
-from nav_msgs.msg import Odometry
+import argparse as ap
 
 import rospy
-import actionlib
-from control_msgs.msg import FollowJointTrajectoryAction
-from control_msgs.msg import FollowJointTrajectoryGoal
-from trajectory_msgs.msg import JointTrajectoryPoint
-
-from sensor_msgs.msg import PointCloud2
-
-from control_msgs.msg import GripperCommandAction
-from control_msgs.msg import GripperCommandGoal
-
 from std_srvs.srv import Trigger, TriggerRequest
-
-import math
-import time
-import threading
-import sys
-
-import tf2_ros
-import argparse as ap
+from sensor_msgs.msg import JointState
 
 import hello_helpers.hello_misc as hm
 
@@ -43,10 +22,10 @@ class GetKeyboardCommands:
         self.deliver_object_on = deliver_object_on
         
         self.step_size = 'medium'
-        self.persistent_command_count = 0
-        self.prev_persistent_c = None
-        self.persistent_start_s = time.time()
-        self.max_persistent_delay_s = 1.0
+        # self.persistent_command_count = 0
+        # self.prev_persistent_c = None
+        # self.persistent_start_s = time.time()
+        # self.max_persistent_delay_s = 1.0
         self.rad_per_deg = math.pi/180.0
         self.small_deg = 3.0
         self.small_rad = self.rad_per_deg * self.small_deg
@@ -220,7 +199,6 @@ class GetKeyboardCommands:
         if c == 'q' or c == 'Q':
             rospy.loginfo('keyboard_teleop exiting...')
             rospy.signal_shutdown('Received quit character (q), so exiting')
-            sys.exit()
 
         return command
         
diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver
index cfce9c6..3a3f172 100755
--- a/stretch_core/nodes/stretch_driver
+++ b/stretch_core/nodes/stretch_driver
@@ -1,43 +1,29 @@
 #! /usr/bin/env python
 
-from __future__ import print_function, division
-
+import yaml
+import numpy as np
 import threading
+import stretch_body.robot as rb
+from stretch_body.hello_utils import ThreadServiceExit
+
+import tf2_ros
+import tf_conversions
 
 import rospy
 from geometry_msgs.msg import Quaternion
-from geometry_msgs.msg import PoseWithCovarianceStamped
 from geometry_msgs.msg import Twist
 from geometry_msgs.msg import TransformStamped
 
-from nav_msgs.msg import Odometry
-from sensor_msgs.msg import JointState, Imu, MagneticField
-from std_msgs.msg import Header
-
-import tf2_ros
-import tf_conversions
-
-from stretch_body.hello_utils import ThreadServiceExit
-import stretch_body.robot as rb
-import math
-import numpy as np
-import time
-
-import yaml
-
 import actionlib
 from control_msgs.msg import FollowJointTrajectoryAction
 from control_msgs.msg import FollowJointTrajectoryResult
-from control_msgs.msg import FollowJointTrajectoryResult
-from trajectory_msgs.msg import JointTrajectoryPoint
-from actionlib_msgs.msg import GoalStatus
-
-from control_msgs.msg import GripperCommandAction
-from control_msgs.msg import GripperCommandFeedback
-from control_msgs.msg import GripperCommandActionResult
 
 from std_srvs.srv import Trigger, TriggerResponse
 
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import JointState, Imu, MagneticField
+from std_msgs.msg import Header
+
 import hello_helpers.hello_misc as hm
 
 
@@ -437,8 +423,8 @@ class MobileBaseCommandGroup:
         self.mobile_base_virtual_joint_ros_range = [-0.5, 0.5]
         self.acceptable_mobile_base_error_m = 0.005
         self.excellent_mobile_base_error_m = 0.005
-        self.acceptable_mobile_base_error_rad = (math.pi/180.0) * 6.0
-        self.excellent_mobile_base_error_rad = (math.pi/180.0) * 0.6
+        self.acceptable_mobile_base_error_rad = (np.pi/180.0) * 6.0
+        self.excellent_mobile_base_error_rad = (np.pi/180.0) * 0.6
 
     def update(self, joint_names, invalid_joints_error_func, robot_mode):
         self.use_mobile_base_virtual_joint = False
@@ -576,7 +562,7 @@ class MobileBaseCommandGroup:
                 # Use velocity to help decide when the low-level command
                 # has been finished.
                 min_deg_per_s = 1.0
-                min_rad_per_s = min_deg_per_s * (math.pi/180.0)
+                min_rad_per_s = min_deg_per_s * (np.pi/180.0)
                 self.mobile_base_goal_reached = self.mobile_base_goal_reached and (abs(robot_status['base']['theta_vel']) < min_rad_per_s)
         return self.mobile_base_error_m, self.mobile_base_error_rad
     
@@ -1267,7 +1253,7 @@ class StretchBodyNode:
 
             rospy.loginfo('controller parameters loaded = {0}'.format(controller_parameters))
 
-            deg_per_rad = 180.0/math.pi
+            deg_per_rad = 180.0/np.pi
             self.head_tilt_calibrated_offset_rad = controller_parameters['tilt_angle_offset']
             ang = self.head_tilt_calibrated_offset_rad
             if (abs(ang) > large_ang):