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


robotiq_c_model_control
Author(s): Nicolas Lauzier (Robotiq inc.)
autogenerated on Thu Jun 6 2019 17:57:59