diff --git a/src/joint_state_printer.py b/src/joint_state_printer.py new file mode 100755 index 0000000..82fb2f0 --- /dev/null +++ b/src/joint_state_printer.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + + +# Import ROS Python basic API and sys +import rospy +import sys + +# We're going to subscribe to 64-bit integers, so we need to import the defintion +# for them. +from sensor_msgs.msg import JointState + + +# This is a function that is called whenever a new message is received. The +# message is passed to the function as a parameter. +def callback(msg): + """ + Callback function to deal with incoming messages. + :param msg: The JointState message. + """ + names = list(msg.name) + positions = list(msg.position) + # The value of the integer is stored in the data attribute of the message. + joints = ["joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_lift" ] + + joint_positions = [] + + for i in range(len(joints)): + index = names.index(joints[i]) + joint_positions.append(positions[index]) + + print(joint_positions) + rospy.signal_shutdown("done") + sys.exit(0) + + + +if __name__ == '__main__': + # Initialize the node. + rospy.init_node('joint_sate_printer', anonymous=True) + + # Set up a subscriber. We're going to subscribe to the topic "counter", + # looking for Int64 messages. When a message comes in, ROS is going to pass + # it to the function "callback" automatically. + # for i in range() + subscriber = rospy.Subscriber('joint_states', JointState, callback) + + # 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. + rospy.spin()