$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 # This program is a simple script used for demos. It runs the hand 00020 # through different given poses. 00021 00022 # It is a good example on how to use the latching feature of the 00023 # ros publisher to make sure a data is received even if we don't stream 00024 # the data. 00025 00026 import roslib; roslib.load_manifest('sr_hand') 00027 import rospy 00028 from sr_robot_msgs.msg import sendupdate, joint 00029 import time 00030 00031 class Pose(object): 00032 def __init__(self, joints_dict): 00033 joints_table = [] 00034 for name in joints_dict.keys(): 00035 joints_table.append(joint(joint_name = name, 00036 joint_target = joints_dict[name])) 00037 00038 self.msg = sendupdate(len(joints_table), joints_table) 00039 00040 00041 00042 class SexyExampleLatching(object): 00043 def __init__(self): 00044 # Here we initialize the publisher with the latch set to True. 00045 # this will ensure that the hand gets the message, even though we're 00046 # using the messages more as a service (we don't stream the data, we 00047 # just ask the hand to take a given position) 00048 self.publisher = rospy.Publisher('/srh/sendupdate', sendupdate, latch=True) 00049 00050 self.sleep_time = 2.0 00051 00052 def run(self): 00053 """ 00054 Runs the hand through different predefined position in a given order. 00055 """ 00056 start = {"FFJ0":0, "FFJ3":0, "FFJ4":0, 00057 "MFJ0":0, "MFJ3":0, "MFJ4":0, 00058 "RFJ0":0, "RFJ3":0, "RFJ4":0, 00059 "LFJ0":0, "LFJ3":0, "LFJ4":0, "LFJ5":0, 00060 "THJ1":0, "THJ2":0, "THJ3":0, "THJ4":0, "THJ5":0, 00061 "WRJ1":0, "WRJ2":0 } 00062 00063 fist = {"FFJ0":180, "FFJ3":90, "FFJ4":0, 00064 "MFJ0":180, "MFJ3":90, "MFJ4":0, 00065 "RFJ0":180, "RFJ3":90, "RFJ4":0, 00066 "LFJ0":180, "LFJ3":90, "LFJ4":0, "LFJ5":0, 00067 "THJ1":40, "THJ2":20, "THJ3":0, "THJ4":50, "THJ5":35, 00068 "WRJ1":0, "WRJ2":0 } 00069 00070 victory = {"FFJ0":0, "FFJ3":0, "FFJ4":-25, 00071 "MFJ0":0, "MFJ3":0, "MFJ4":25, 00072 "RFJ0":180, "RFJ3":90, "RFJ4":-25, 00073 "LFJ0":180, "LFJ3":90, "LFJ4":-25, "LFJ5":0, 00074 "THJ1":40, "THJ2":20, "THJ3":0, "THJ4":50, "THJ5":35, 00075 "WRJ1":0, "WRJ2":0 } 00076 00077 wave_1 = {"WRJ2":-30} 00078 wave_2 = {"WRJ2":10} 00079 00080 00081 self.publish_pose(Pose(start)) 00082 time.sleep(self.sleep_time) 00083 self.publish_pose(Pose(fist)) 00084 time.sleep(self.sleep_time) 00085 00086 self.publish_pose(Pose(start)) 00087 time.sleep(self.sleep_time) 00088 00089 self.publish_pose(Pose(victory)) 00090 time.sleep(self.sleep_time) 00091 00092 self.publish_pose(Pose(start)) 00093 time.sleep(self.sleep_time) 00094 00095 for i in range(0,4): 00096 self.publish_pose(Pose(wave_1)) 00097 time.sleep(self.sleep_time) 00098 self.publish_pose(Pose(wave_2)) 00099 time.sleep(self.sleep_time) 00100 00101 00102 def publish_pose(self, pose): 00103 """ 00104 Publish a given pose. 00105 """ 00106 self.publisher.publish(pose.msg) 00107 00108 def main(): 00109 rospy.init_node('sexy_example_latching', anonymous = True) 00110 sexy_example = SexyExampleLatching() 00111 sexy_example.run() 00112 00113 if __name__ == '__main__': 00114 main()