SModelSimpleController.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 
00004 # Software License Agreement (BSD License)
00005 #
00006 # Copyright (c) 2012, Robotiq, Inc.
00007 # All rights reserved.
00008 #
00009 # Redistribution and use in source and binary forms, with or without
00010 # modification, are permitted provided that the following conditions
00011 # are met:
00012 #
00013 #  * Redistributions of source code must retain the above copyright
00014 #    notice, this list of conditions and the following disclaimer.
00015 #  * Redistributions in binary form must reproduce the above
00016 #    copyright notice, this list of conditions and the following
00017 #    disclaimer in the documentation and/or other materials provided
00018 #    with the distribution.
00019 #  * Neither the name of Robotiq, Inc. nor the names of its
00020 #    contributors may be used to endorse or promote products derived
00021 #    from this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 # POSSIBILITY OF SUCH DAMAGE.
00035 #
00036 # Copyright (c) 2012, Robotiq, Inc.
00037 # Revision $Id$
00038 
00039 """@package docstring
00040 Command-line interface for sending simple commands to a ROS node controlling a S-Model gripper.
00041 
00042 This serves as an example for publishing messages on the 'SModelRobotOutput' topic using the 'SModel_robot_output' msg type for sending commands to a S-Model gripper. In this example, only the simple control mode is implemented. For using the advanced control mode, please refer to the Robotiq support website (support.robotiq.com).
00043 """
00044 
00045 import roslib; roslib.load_manifest('robotiq_s_model_control')
00046 import rospy
00047 from robotiq_s_model_control.msg import _SModel_robot_output  as outputMsg
00048 from time import sleep
00049 
00050 
00051 def genCommand(char, command):
00052     """Update the command according to the character entered by the user."""    
00053         
00054     if char == 'a':
00055         command = outputMsg.SModel_robot_output();
00056         command.rACT = 1
00057         command.rGTO = 1
00058         command.rSPA = 255
00059         command.rFRA = 150
00060 
00061     if char == 'r':
00062         command = outputMsg.SModel_robot_output();
00063         command.rACT = 0
00064 
00065     if char == 'c':
00066         command.rPRA = 255
00067 
00068     if char == 'o':
00069         command.rPRA = 0
00070 
00071     if char == 'b':
00072         command.rMOD = 0
00073         
00074     if char == 'p':
00075         command.rMOD = 1
00076         
00077     if char == 'w':
00078         command.rMOD = 2
00079         
00080     if char == 's':
00081         command.rMOD = 3
00082 
00083     #If the command entered is a int, assign this value to rPRA
00084     try: 
00085         command.rPRA = int(char)
00086         if command.rPRA > 255:
00087             command.rPRA = 255
00088         if command.rPRA < 0:
00089             command.rPRA = 0
00090     except ValueError:
00091         pass                    
00092         
00093     if char == 'f':
00094         command.rSPA += 25
00095         if command.rSPA > 255:
00096             command.rSPA = 255
00097             
00098     if char == 'l':
00099         command.rSPA -= 25
00100         if command.rSPA < 0:
00101             command.rSPA = 0
00102 
00103             
00104     if char == 'i':
00105         command.rFRA += 25
00106         if command.rFRA > 255:
00107             command.rFRA = 255
00108             
00109     if char == 'd':
00110         command.rFRA -= 25
00111         if command.rFRA < 0:
00112             command.rFRA = 0
00113 
00114     return command
00115         
00116 
00117 def askForCommand(command):
00118     """Ask the user for a command to send to the gripper."""    
00119 
00120     currentCommand  = 'Simple S-Model Controller\n-----\nCurrent command:'
00121     currentCommand += ' rACT = '  + str(command.rACT)
00122     currentCommand += ', rMOD = ' + str(command.rMOD)
00123     currentCommand += ', rGTO = ' + str(command.rGTO)
00124     currentCommand += ', rATR = ' + str(command.rATR)
00125 ##    currentCommand += ', rGLV = ' + str(command.rGLV)
00126 ##    currentCommand += ', rICF = ' + str(command.rICF)
00127 ##    currentCommand += ', rICS = ' + str(command.rICS)
00128     currentCommand += ', rPRA = ' + str(command.rPRA)
00129     currentCommand += ', rSPA = ' + str(command.rSPA)
00130     currentCommand += ', rFRA = ' + str(command.rFRA)
00131 
00132     #We only show the simple control mode
00133 ##    currentCommand += ', rPRB = ' + str(command.rPRB)
00134 ##    currentCommand += ', rSPB = ' + str(command.rSPB)
00135 ##    currentCommand += ', rFRB = ' + str(command.rFRB)
00136 ##    currentCommand += ', rPRC = ' + str(command.rPRC)
00137 ##    currentCommand += ', rSPC = ' + str(command.rSPC)
00138 ##    currentCommand += ', rFRC = ' + str(command.rFRC)
00139 ##    currentCommand += ', rPRS = ' + str(command.rPRS)
00140 ##    currentCommand += ', rSPS = ' + str(command.rSPS)
00141 ##    currentCommand += ', rFRS = ' + str(command.rFRS)
00142 
00143     print currentCommand
00144 
00145     strAskForCommand  = '-----\nAvailable commands\n\n'
00146     strAskForCommand += 'r: Reset\n'
00147     strAskForCommand += 'a: Activate\n'
00148     strAskForCommand += 'c: Close\n'
00149     strAskForCommand += 'o: Open\n'
00150     strAskForCommand += 'b: Basic mode\n'
00151     strAskForCommand += 'p: Pinch mode\n'
00152     strAskForCommand += 'w: Wide mode\n'
00153     strAskForCommand += 's: Scissor mode\n'
00154     strAskForCommand += '(0-255): Go to that position\n'
00155     strAskForCommand += 'f: Faster\n'
00156     strAskForCommand += 'l: Slower\n'
00157     strAskForCommand += 'i: Increase force\n'
00158     strAskForCommand += 'd: Decrease force\n'
00159     
00160     strAskForCommand += '-->'
00161 
00162     return raw_input(strAskForCommand)
00163 
00164 def publisher():
00165     """Main loop which requests new commands and publish them on the SModelRobotOutput topic."""
00166 
00167     rospy.init_node('SModelSimpleController')
00168     
00169     pub = rospy.Publisher('SModelRobotOutput', outputMsg.SModel_robot_output)
00170 
00171     command = outputMsg.SModel_robot_output();
00172 
00173     while not rospy.is_shutdown():
00174 
00175         command = genCommand(askForCommand(command), command)            
00176         
00177         pub.publish(command)
00178 
00179         rospy.sleep(0.1)
00180                         
00181 
00182 if __name__ == '__main__':
00183     publisher()


robotiq_s_model_control
Author(s): Nicolas Lauzier (Robotiq inc.)
autogenerated on Thu Aug 27 2015 14:44:26