Add HelloNode.get_joint_state() + other improvements

The get_joint_state() method makes it easier to get
information about a joint, including whether the
joint is moving. A mode variable is added to make
it easier to determine what mode the driver is in.
Lastly, some of HelloNode's variables are made private
to make it easier to use the class from iPython.

hello_helpers/src/hello_helpers/hello_misc.py

@@ -18,7 +18,7 @@ from control_msgs.msg import FollowJointTrajectoryAction
 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 import tf2_ros
-from sensor_msgs.msg import PointCloud2
+from sensor_msgs.msg import PointCloud2, JointState
 from std_srvs.srv import Trigger, TriggerRequest
 from std_msgs.msg import String
 
@@ -68,9 +68,10 @@ def get_left_finger_state(joint_states):
 
 class HelloNode:
     def __init__(self):
-        self.joint_state = None
+        self.joint_states = None
         self.point_cloud = None
         self.tool = None
+        self.mode = None
         self.dryrun = False
 
     @classmethod
@@ -79,15 +80,18 @@ class HelloNode:
         i.main(name, name, wait_for_first_pointcloud)
         return i
 
-    def joint_states_callback(self, joint_state):
-        self.joint_state = joint_state
+    def _joint_states_callback(self, joint_states):
+        self.joint_states = joint_states
 
-    def point_cloud_callback(self, point_cloud):
+    def _point_cloud_callback(self, point_cloud):
         self.point_cloud = point_cloud
 
-    def tool_callback(self, tool_string):
+    def _tool_callback(self, tool_string):
         self.tool = tool_string.data
 
+    def _mode_callback(self, mode_string):
+        self.mode = mode_string.data
+
     def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False):
         if self.dryrun:
             return
@@ -201,6 +205,18 @@ class HelloNode:
         assert(self.tool is not None)
         return self.tool
 
+    def get_mode(self):
+        assert(self.mode is not None)
+        return self.mode
+
+    def get_joint_state(self, joint_name, moving_threshold=0.001):
+        i = self.joint_states.name.index(joint_name)
+        joint_position = self.joint_states.position[i]
+        joint_velocity = self.joint_states.velocity[i]
+        joint_effort = self.joint_states.effort[i]
+        joint_is_moving = abs(joint_velocity) > moving_threshold
+        return (joint_position, joint_velocity, joint_effort, joint_is_moving)
+
     def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
         rospy.init_node(node_name)
         self.node_name = rospy.get_name()
@@ -211,13 +227,16 @@ class HelloNode:
         if not server_reached:
             rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.')
             sys.exit()
-        
+
+        self._joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self._joint_states_callback)
+
         self.tf2_buffer = tf2_ros.Buffer()
         self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
 
-        self.tool_subscriber = rospy.Subscriber('/tool', String, self.tool_callback)
+        self._tool_subscriber = rospy.Subscriber('/tool', String, self._tool_callback)
+        self._mode_subscriber = rospy.Subscriber('/mode', String, self._mode_callback)
 
-        self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback)
+        self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self._point_cloud_callback)
         self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1)
 
         rospy.wait_for_service('/home_the_robot')