example_reading_sensors.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('sr_edc_muscle_tools')
import rospy
from std_msgs.msg import Float64
from sr_robot_msgs.msg import JointMusclePositionControllerState
import math

# A callback function for the state messages we are reading.
status = None
def state_cb(data):
    global status
    status = data

rospy.init_node('example_reading_sensors')

# Set up the subscriber so we can read the data, each joint publishes its own state.
rospy.Subscriber('/sh_ffj3_muscle_position_controller/state',
                 JointMusclePositionControllerState, state_cb)

print "Try moving ffj3"
r = rospy.Rate(1) # hz
while not rospy.is_shutdown():
   if status:
        print "ffj3 position:%s radians  pressure0:%s pressure1:%s"%(status.process_value, status.muscle_pressure_0, status.muscle_pressure_1)
   r.sleep()