From 3574fc2aa96786d6a668a0a2f5f20716c4677bb6 Mon Sep 17 00:00:00 2001
From: Jesus Eduardo Rodriguez <141784078+hello-jesus@users.noreply.github.com>
Date: Thu, 7 Sep 2023 15:27:47 -0700
Subject: [PATCH] Created and tested new ROS2 tutorial for the joint state
 printer

---
 stretch_ros_tutorials/joint_state_printer.py | 89 ++++++++++++++++++++
 1 file changed, 89 insertions(+)
 create mode 100644 stretch_ros_tutorials/joint_state_printer.py

diff --git a/stretch_ros_tutorials/joint_state_printer.py b/stretch_ros_tutorials/joint_state_printer.py
new file mode 100644
index 0000000..bdb27e3
--- /dev/null
+++ b/stretch_ros_tutorials/joint_state_printer.py
@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import sys
+import time
+
+# We're going to subscribe to a JointState message type, so we need to import
+# the definition for it
+from sensor_msgs.msg import JointState
+
+class JointStatePublisher(Node):
+	"""
+	A class that prints the positions of desired joints in Stretch.
+	"""
+
+	def __init__(self):
+		"""
+		Function that initializes the subscriber.
+		:param self: The self reference
+		"""
+		super().__init__('stretch_joint_state')
+		
+        # Set up a subscriber. We're going to subscribe to the topic "joint_states"
+		self.sub = self.create_subscription(JointState, 'joint_states', self.callback, 1)
+
+
+	def callback(self, msg):
+		"""
+		Callback function to deal with the incoming JointState messages.
+		:param self: The self reference.
+		:param msg: The JointState message.
+		"""
+		# Store the joint messages for later use
+		self.get_logger().info('Receiving JointState messages')
+		self.joint_states = msg
+
+
+	def print_states(self, joints):
+		"""
+		print_states function to deal with the incoming JointState messages.
+		:param self: The self reference.
+		:param joints: A list of string values of joint names.
+		"""
+		# Create an empty list that will store the positions of the requested joints
+		joint_positions = []
+
+		# Use of forloop to parse the names of the requested joints list.
+		# The index() function returns the index at the first occurrence of
+		# the name of the requested joint in the self.joint_states.name list
+		for joint in joints:
+			if joint == "wrist_extension":
+				index = self.joint_states.name.index('joint_arm_l0')
+				joint_positions.append(4*self.joint_states.position[index])
+				continue
+			
+			index = self.joint_states.name.index(joint)
+			joint_positions.append(self.joint_states.position[index])
+
+		# Print the joint position values to the terminal
+		print("name: " + str(joints))
+		print("position: " + str(joint_positions))
+
+		# Sends a signal to rclpy to shutdown the ROS interfaces
+		rclpy.shutdown()
+
+		# Exit the Python interpreter
+		sys.exit(0)
+
+def main(args=None):
+	# Initialize the node
+    rclpy.init(args=args)
+    joint_publisher = JointStatePublisher()
+    time.sleep(1)
+    rclpy.spin_once(joint_publisher)
+
+	# Create a list of the joints and name them joints. These will be an argument
+	# for the print_states() function
+    joints = ["joint_lift", "wrist_extension", "joint_wrist_yaw"]
+    joint_publisher.print_states(joints)
+    
+    # Give control to ROS.  This will allow the callback to be called whenever new
+	# messages come in.  If we don't put this line in, then the node will not work,
+	# and ROS will not process any messages
+    rclpy.spin(joint_publisher)
+
+
+if __name__ == '__main__':
+	main()