$search
00001 #!/usr/bin/env python 00002 # 00003 # Copyright 2011 Shadow Robot Company Ltd. 00004 # 00005 # This program is free software: you can redistribute it and/or modify it 00006 # under the terms of the GNU General Public License as published by the Free 00007 # Software Foundation, either version 2 of the License, or (at your option) 00008 # any later version. 00009 # 00010 # This program is distributed in the hope that it will be useful, but WITHOUT 00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 00012 # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 00013 # more details. 00014 # 00015 # You should have received a copy of the GNU General Public License along 00016 # with this program. If not, see <http://www.gnu.org/licenses/>. 00017 # 00018 00019 """ 00020 This is a simple subscriber example, subscribing to the srh/shadowhand_data topic 00021 """ 00022 00023 import roslib; roslib.load_manifest('sr_hand') 00024 import rospy 00025 from sr_robot_msgs.msg import joints_data, joint 00026 00027 def callback(joint_data): 00028 """ 00029 The callback function for the topic srh/shadowhand_Data. It just displays the received information on the console. 00030 00031 @param joint_data: the message containing the joint data. 00032 """ 00033 for joint in joint_data.joints_list: 00034 rospy.loginfo(rospy.get_name()+"[%s] : Pos = %f | Target = %f | Temp = %f | Current = %f", 00035 joint.joint_name, joint.joint_position, joint.joint_target, joint.joint_temperature, 00036 joint.joint_current) 00037 00038 def listener(): 00039 """ 00040 Initialize the ROS node and the topic to which it subscribes. 00041 """ 00042 rospy.init_node('shadowhand_subscriber_python', anonymous=True) 00043 rospy.Subscriber("srh/shadowhand_data", joints_data, callback) 00044 rospy.spin() 00045 00046 if __name__ == '__main__': 00047 listener()