diff --git a/robotiq_2f_gripper_control/nodes/Robotiq2FGripperRtuNode.py b/robotiq_2f_gripper_control/nodes/Robotiq2FGripperRtuNode.py index 06e71fea..938c1db4 100755 --- a/robotiq_2f_gripper_control/nodes/Robotiq2FGripperRtuNode.py +++ b/robotiq_2f_gripper_control/nodes/Robotiq2FGripperRtuNode.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Software License Agreement (BSD License) # @@ -51,7 +51,7 @@ from robotiq_2f_gripper_control.msg import _Robotiq2FGripper_robot_output as outputMsg def mainLoop(device): - + #Gripper is a 2F with a TCP connection gripper = robotiq_2f_gripper_control.baseRobotiq2FGripper.robotiqbaseRobotiq2FGripper() gripper.client = robotiq_modbus_rtu.comModbusRtu.communication() @@ -66,14 +66,14 @@ def mainLoop(device): #The Gripper command is received from the topic named 'Robotiq2FGripperRobotOutput' rospy.Subscriber('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, gripper.refreshCommand) - + #We loop while not rospy.is_shutdown(): #Get and publish the Gripper status status = gripper.getStatus() - pub.publish(status) + pub.publish(status) #Wait a little #rospy.sleep(0.05) @@ -83,7 +83,7 @@ def mainLoop(device): #Wait a little #rospy.sleep(0.05) - + if __name__ == '__main__': try: mainLoop(sys.argv[1]) diff --git a/robotiq_2f_gripper_control/nodes/Robotiq2FGripperSimpleController.py b/robotiq_2f_gripper_control/nodes/Robotiq2FGripperSimpleController.py index 3294a0fa..ea29646a 100755 --- a/robotiq_2f_gripper_control/nodes/Robotiq2FGripperSimpleController.py +++ b/robotiq_2f_gripper_control/nodes/Robotiq2FGripperSimpleController.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Software License Agreement (BSD License) # @@ -41,15 +41,21 @@ This serves as an example for publishing messages on the 'Robotiq2FGripperRobotOutput' topic using the 'Robotiq2FGripper_robot_output' msg type for sending commands to a 2F gripper. """ +from __future__ import print_function + import roslib; roslib.load_manifest('robotiq_2f_gripper_control') import rospy from robotiq_2f_gripper_control.msg import _Robotiq2FGripper_robot_output as outputMsg from time import sleep +try: + input = raw_input +except NameError: + pass def genCommand(char, command): - """Update the command according to the character entered by the user.""" - + """Update the command according to the character entered by the user.""" + if char == 'a': command = outputMsg.Robotiq2FGripper_robot_output(); command.rACT = 1 @@ -65,44 +71,44 @@ def genCommand(char, command): command.rPR = 255 if char == 'o': - command.rPR = 0 + command.rPR = 0 #If the command entered is a int, assign this value to rPRA - try: + try: command.rPR = int(char) if command.rPR > 255: command.rPR = 255 if command.rPR < 0: command.rPR = 0 except ValueError: - pass - + pass + if char == 'f': command.rSP += 25 if command.rSP > 255: command.rSP = 255 - + if char == 'l': command.rSP -= 25 if command.rSP < 0: command.rSP = 0 - + if char == 'i': command.rFR += 25 if command.rFR > 255: command.rFR = 255 - + if char == 'd': command.rFR -= 25 if command.rFR < 0: command.rFR = 0 return command - + def askForCommand(command): - """Ask the user for a command to send to the gripper.""" + """Ask the user for a command to send to the gripper.""" currentCommand = 'Simple 2F Gripper Controller\n-----\nCurrent command:' currentCommand += ' rACT = ' + str(command.rACT) @@ -113,7 +119,7 @@ def askForCommand(command): currentCommand += ', rFR = ' + str(command.rFR ) - print currentCommand + print(currentCommand) strAskForCommand = '-----\nAvailable commands\n\n' strAskForCommand += 'r: Reset\n' @@ -125,27 +131,27 @@ def askForCommand(command): strAskForCommand += 'l: Slower\n' strAskForCommand += 'i: Increase force\n' strAskForCommand += 'd: Decrease force\n' - + strAskForCommand += '-->' - return raw_input(strAskForCommand) + return input(strAskForCommand) def publisher(): """Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic.""" rospy.init_node('Robotiq2FGripperSimpleController') - + pub = rospy.Publisher('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) command = outputMsg.Robotiq2FGripper_robot_output(); while not rospy.is_shutdown(): - command = genCommand(askForCommand(command), command) - + command = genCommand(askForCommand(command), command) + pub.publish(command) rospy.sleep(0.1) - + if __name__ == '__main__': publisher() diff --git a/robotiq_2f_gripper_control/nodes/Robotiq2FGripperStatusListener.py b/robotiq_2f_gripper_control/nodes/Robotiq2FGripperStatusListener.py index 2a8486e5..bd2b2004 100755 --- a/robotiq_2f_gripper_control/nodes/Robotiq2FGripperStatusListener.py +++ b/robotiq_2f_gripper_control/nodes/Robotiq2FGripperStatusListener.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Software License Agreement (BSD License) # @@ -41,6 +41,8 @@ This serves as an example for receiving messages from the 'Robotiq2FGripperRobotInput' topic using the 'Robotiq2FGripper_robot_input' msg type and interpreting the corresponding status of the 2F gripper. """ +from __future__ import print_function + import roslib; roslib.load_manifest('robotiq_2f_gripper_control') import rospy from std_msgs.msg import String @@ -49,7 +51,7 @@ def printStatus(status): """Print the status string generated by the statusInterpreter function.""" - print statusInterpreter(status) + print(statusInterpreter(status)) def Robotiq2FGripperStatusListener(): """Initialize the node and subscribe to the Robotiq2FGripperRobotInput topic.""" @@ -98,7 +100,7 @@ def statusInterpreter(status): output += 'Fingers have stopped due to a contact while closing \n' if(status.gOBJ == 3): output += 'Fingers are at requested position\n' - + #gFLT output += 'gFLT = ' + str(status.gFLT) + ': ' if(status.gFLT == 0x00): @@ -108,7 +110,7 @@ def statusInterpreter(status): if(status.gFLT == 0x07): output += 'Priority Fault: The activation bit must be set prior to action\n' if(status.gFLT == 0x09): - output += 'Minor Fault: The communication chip is not ready (may be booting)\n' + output += 'Minor Fault: The communication chip is not ready (may be booting)\n' if(status.gFLT == 0x0B): output += 'Minor Fault: Automatic release in progress\n' if(status.gFLT == 0x0E): @@ -126,9 +128,9 @@ def statusInterpreter(status): #gCU output += 'gCU = ' + str(status.gCU) + ': ' - output += 'Current of Fingers: ' + str(status.gCU * 10) + ' mA\n' + output += 'Current of Fingers: ' + str(status.gCU * 10) + ' mA\n' return output - + if __name__ == '__main__': Robotiq2FGripperStatusListener() diff --git a/robotiq_2f_gripper_control/nodes/Robotiq2FGripperTcpNode.py b/robotiq_2f_gripper_control/nodes/Robotiq2FGripperTcpNode.py index 01ec1fc7..d9f22dac 100755 --- a/robotiq_2f_gripper_control/nodes/Robotiq2FGripperTcpNode.py +++ b/robotiq_2f_gripper_control/nodes/Robotiq2FGripperTcpNode.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Software License Agreement (BSD License) # @@ -51,7 +51,7 @@ from robotiq_2f_gripper_control.msg import _Robotiq2FGripper_robot_output as outputMsg def mainLoop(address): - + #Gripper is a 2F with a TCP connection gripper = robotiq_2f_gripper_control.baseRobotiq2FGripper.robotiqbaseRobotiq2FGripper() gripper.client = robotiq_modbus_tcp.comModbusTcp.communication() @@ -66,14 +66,14 @@ def mainLoop(address): #The Gripper command is received from the topic named 'Robotiq2FGripperRobotOutput' rospy.Subscriber('Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output, gripper.refreshCommand) - + #We loop while not rospy.is_shutdown(): #Get and publish the Gripper status status = gripper.getStatus() - pub.publish(status) + pub.publish(status) #Wait a little rospy.sleep(0.05) @@ -83,7 +83,7 @@ def mainLoop(address): #Wait a little rospy.sleep(0.05) - + if __name__ == '__main__': try: #TODO: Add verification that the argument is an IP address diff --git a/robotiq_2f_gripper_control/src/robotiq_2f_gripper_control/baseRobotiq2FGripper.py b/robotiq_2f_gripper_control/src/robotiq_2f_gripper_control/baseRobotiq2FGripper.py index 6fd5bed1..8e2f52ad 100644 --- a/robotiq_2f_gripper_control/src/robotiq_2f_gripper_control/baseRobotiq2FGripper.py +++ b/robotiq_2f_gripper_control/src/robotiq_2f_gripper_control/baseRobotiq2FGripper.py @@ -54,34 +54,34 @@ def __init__(self): def verifyCommand(self, command): """Function to verify that the value of each variable satisfy its limits.""" - - #Verify that each variable is in its correct range - command.rACT = max(0, command.rACT) - command.rACT = min(1, command.rACT) - - command.rGTO = max(0, command.rGTO) - command.rGTO = min(1, command.rGTO) - - command.rATR = max(0, command.rATR) - command.rATR = min(1, command.rATR) - - command.rPR = max(0, command.rPR) - command.rPR = min(255, command.rPR) - - command.rSP = max(0, command.rSP) - command.rSP = min(255, command.rSP) - - command.rFR = max(0, command.rFR) - command.rFR = min(255, command.rFR) - - #Return the modified command - return command + + #Verify that each variable is in its correct range + command.rACT = max(0, command.rACT) + command.rACT = min(1, command.rACT) + + command.rGTO = max(0, command.rGTO) + command.rGTO = min(1, command.rGTO) + + command.rATR = max(0, command.rATR) + command.rATR = min(1, command.rATR) + + command.rPR = max(0, command.rPR) + command.rPR = min(255, command.rPR) + + command.rSP = max(0, command.rSP) + command.rSP = min(255, command.rSP) + + command.rFR = max(0, command.rFR) + command.rFR = min(255, command.rFR) + + #Return the modified command + return command def refreshCommand(self, command): """Function to update the command which will be sent during the next sendCommand() call.""" - - #Limit the value of each variable - command = self.verifyCommand(command) + + #Limit the value of each variable + command = self.verifyCommand(command) #Initiate command as an empty list self.message = [] @@ -93,11 +93,11 @@ def refreshCommand(self, command): self.message.append(0) self.message.append(command.rPR) self.message.append(command.rSP) - self.message.append(command.rFR) + self.message.append(command.rFR) def sendCommand(self): - """Send the command to the Gripper.""" - + """Send the command to the Gripper.""" + self.client.sendCommand(self.message) def getStatus(self): @@ -110,14 +110,14 @@ def getStatus(self): message = inputMsg.Robotiq2FGripper_robot_input() #Assign the values to their respective variables - message.gACT = (status[0] >> 0) & 0x01; + message.gACT = (status[0] >> 0) & 0x01; message.gGTO = (status[0] >> 3) & 0x01; message.gSTA = (status[0] >> 4) & 0x03; message.gOBJ = (status[0] >> 6) & 0x03; message.gFLT = status[2] message.gPR = status[3] message.gPO = status[4] - message.gCU = status[5] + message.gCU = status[5] return message - + diff --git a/robotiq_2f_gripper_control/src/robotiq_2f_gripper_control/robotiq_2f_gripper_ctrl.py b/robotiq_2f_gripper_control/src/robotiq_2f_gripper_control/robotiq_2f_gripper_ctrl.py index 4c0318e1..8d32a7e1 100755 --- a/robotiq_2f_gripper_control/src/robotiq_2f_gripper_control/robotiq_2f_gripper_ctrl.py +++ b/robotiq_2f_gripper_control/src/robotiq_2f_gripper_control/robotiq_2f_gripper_ctrl.py @@ -1,4 +1,6 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 + +from __future__ import print_function import numpy as np @@ -166,12 +168,12 @@ def main(): if gripper.is_reset(): gripper.reset() gripper.activate() - print gripper.close(block=True) + print(gripper.close(block=True)) while not rospy.is_shutdown(): - print gripper.open(block=False) + print(gripper.open(block=False)) rospy.sleep(0.11) gripper.stop() - print gripper.close(block=False) + print(gripper.close(block=False)) rospy.sleep(0.1) gripper.stop() diff --git a/robotiq_3f_gripper_articulated_gazebo_plugins/CMakeLists.txt b/robotiq_3f_gripper_articulated_gazebo_plugins/CMakeLists.txt index 7490a381..7a8f037b 100644 --- a/robotiq_3f_gripper_articulated_gazebo_plugins/CMakeLists.txt +++ b/robotiq_3f_gripper_articulated_gazebo_plugins/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(robotiq_3f_gripper_articulated_gazebo_plugins) -add_compile_options(-std=c++11) +set(CMAKE_CXX_STANDARD 17) find_package(catkin REQUIRED COMPONENTS std_msgs gazebo_plugins actionlib tf image_transport control_msgs trajectory_msgs geometry_msgs sensor_msgs roscpp gazebo_ros robotiq_3f_gripper_articulated_msgs) diff --git a/robotiq_3f_gripper_articulated_gazebo_plugins/src/RobotiqHandPlugin.cpp b/robotiq_3f_gripper_articulated_gazebo_plugins/src/RobotiqHandPlugin.cpp index cd9e1042..1936ab56 100644 --- a/robotiq_3f_gripper_articulated_gazebo_plugins/src/RobotiqHandPlugin.cpp +++ b/robotiq_3f_gripper_articulated_gazebo_plugins/src/RobotiqHandPlugin.cpp @@ -16,7 +16,7 @@ */ /* This file has been modified from the original, by Devon Ash -*/ +*/ #include #include @@ -24,19 +24,10 @@ #include #include -/* - -Due to necessity, I had to change the PID.hh file's definition from private members to public to allow public access of its members. They're private in 1.9 and getter functions aren't implemented until gazebo 3.0. I thought that was silly, and so I hacked around it. The functions directly access the private members by necessity. It can only change if Gazebo patches 1.9 and 2.2 to include getters for it. - -I'm not sure exactly where the dependency chain includes PID.hh for the first time, so I've encapsulated all of the gazebo includes. Not pretty, but it works. If you're reading this and know of a better soln', feel free to change it. - -*/ -#define private public #include #include #include #include -#undef private // Default topic names initialization. const std::string RobotiqHandPlugin::DefaultLeftTopicCommand = @@ -148,7 +139,7 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent, if (this->sdf->HasElement("kd_position")) { this->posePID[i].SetDGain(this->sdf->Get("kd_position")); - std::cout << "dGain after overloading: " << this->posePID[i].dGain + std::cout << "dGain after overloading: " << this->posePID[i].GetDGain() << std::endl; } @@ -227,13 +218,13 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent, { gzlog << "Position PID parameters for joint [" << this->fingerJoints[i]->GetName() << "]:" << std::endl - << "\tKP: " << this->posePID[i].pGain << std::endl - << "\tKI: " << this->posePID[i].iGain << std::endl - << "\tKD: " << this->posePID[i].dGain << std::endl - << "\tIMin: " << this->posePID[i].iMin << std::endl - << "\tIMax: " << this->posePID[i].iMax << std::endl - << "\tCmdMin: " << this->posePID[i].cmdMin << std::endl - << "\tCmdMax: " << this->posePID[i].cmdMax << std::endl + << "\tKP: " << this->posePID[i].GetPGain() << std::endl + << "\tKI: " << this->posePID[i].GetIGain() << std::endl + << "\tKD: " << this->posePID[i].GetDGain() << std::endl + << "\tIMin: " << this->posePID[i].GetIMin() << std::endl + << "\tIMax: " << this->posePID[i].GetIMax() << std::endl + << "\tCmdMin: " << this->posePID[i].GetCmdMin() << std::endl + << "\tCmdMax: " << this->posePID[i].GetCmdMax() << std::endl << std::endl; } gzlog << "Topic for sending hand commands: [" << controlTopicName @@ -270,7 +261,7 @@ bool RobotiqHandPlugin::VerifyCommand( this->VerifyField("rPRB", 0, 255, _command->rACT) && this->VerifyField("rSPB", 0, 255, _command->rACT) && this->VerifyField("rFRB", 0, 255, _command->rACT) && - this->VerifyField("rPRC", 0, 255, _command->rACT) && + this->VerifyField("rPRC", 0, 255, _command->rACT) && this->VerifyField("rSPC", 0, 255, _command->rACT) && this->VerifyField("rFRC", 0, 255, _command->rACT) && this->VerifyField("rPRS", 0, 255, _command->rACT) && diff --git a/robotiq_3f_gripper_control/cfg/Robotiq3FGripper.cfg b/robotiq_3f_gripper_control/cfg/Robotiq3FGripper.cfg index 7c13c303..ffbc01db 100755 --- a/robotiq_3f_gripper_control/cfg/Robotiq3FGripper.cfg +++ b/robotiq_3f_gripper_control/cfg/Robotiq3FGripper.cfg @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright (c) 2016, Toyota Research Institute. All rights reserved. diff --git a/robotiq_3f_gripper_control/nodes/Robotiq3FGripperSimpleController.py b/robotiq_3f_gripper_control/nodes/Robotiq3FGripperSimpleController.py index 20d2d3ce..1f991fe8 100755 --- a/robotiq_3f_gripper_control/nodes/Robotiq3FGripperSimpleController.py +++ b/robotiq_3f_gripper_control/nodes/Robotiq3FGripperSimpleController.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Software License Agreement (BSD License) @@ -50,6 +50,10 @@ import rospy from robotiq_3f_gripper_articulated_msgs.msg import Robotiq3FGripperRobotOutput +try: + input = raw_input +except NameError: + pass def genCommand(char, command): """Update the command according to the character entered by the user.""" @@ -161,7 +165,7 @@ def askForCommand(command): strAskForCommand += '-->' - return raw_input(strAskForCommand) + return input(strAskForCommand) def publisher(): diff --git a/robotiq_3f_gripper_control/nodes/Robotiq3FGripperStatusListener.py b/robotiq_3f_gripper_control/nodes/Robotiq3FGripperStatusListener.py index 7b7faa2d..8eb235a9 100755 --- a/robotiq_3f_gripper_control/nodes/Robotiq3FGripperStatusListener.py +++ b/robotiq_3f_gripper_control/nodes/Robotiq3FGripperStatusListener.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Software License Agreement (BSD License) # diff --git a/robotiq_3f_gripper_control/nodes/Robotiq3FGripperTcpNode.py b/robotiq_3f_gripper_control/nodes/Robotiq3FGripperTcpNode.py index 73a87bb9..511a35f6 100755 --- a/robotiq_3f_gripper_control/nodes/Robotiq3FGripperTcpNode.py +++ b/robotiq_3f_gripper_control/nodes/Robotiq3FGripperTcpNode.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Software License Agreement (BSD License) # diff --git a/robotiq_3f_rviz/CMakeLists.txt b/robotiq_3f_rviz/CMakeLists.txt index 8744a096..5bb8785d 100644 --- a/robotiq_3f_rviz/CMakeLists.txt +++ b/robotiq_3f_rviz/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(robotiq_3f_rviz) ## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_INCLUDE_CURRENT_DIR ON) set(CMAKE_AUTOMOC ON) diff --git a/robotiq_modbus_rtu/package.xml b/robotiq_modbus_rtu/package.xml index 789668fe..e3e3fd2f 100644 --- a/robotiq_modbus_rtu/package.xml +++ b/robotiq_modbus_rtu/package.xml @@ -11,7 +11,7 @@ catkin - python-pymodbus + python3-pymodbus rospy diff --git a/robotiq_modbus_rtu/src/robotiq_modbus_rtu/comModbusRtu.py b/robotiq_modbus_rtu/src/robotiq_modbus_rtu/comModbusRtu.py index d468a5dc..60ae8c94 100644 --- a/robotiq_modbus_rtu/src/robotiq_modbus_rtu/comModbusRtu.py +++ b/robotiq_modbus_rtu/src/robotiq_modbus_rtu/comModbusRtu.py @@ -38,24 +38,26 @@ """@package docstring -Module comModbusRtu: defines a class which communicates with Robotiq Grippers using the Modbus RTU protocol. +Module comModbusRtu: defines a class which communicates with Robotiq Grippers using the Modbus RTU protocol. The module depends on pymodbus (http://code.google.com/p/pymodbus/) for the Modbus RTU client. """ +from __future__ import print_function + from pymodbus.client.sync import ModbusSerialClient from math import ceil -class communication: +class communication: def __init__(self): self.client = None - + def connectToDevice(self, device): """Connection to the client - the method takes the IP address (as a string, e.g. '192.168.1.11') as an argument.""" self.client = ModbusSerialClient(method='rtu',port=device,stopbits=1, bytesize=8, baudrate=115200, timeout=0.2) if not self.client.connect(): - print "Unable to connect to %s" % device + print("Unable to connect to {}".format(device)) return False return True @@ -63,9 +65,9 @@ def disconnectFromDevice(self): """Close connection""" self.client.close() - def sendCommand(self, data): + def sendCommand(self, data): """Send a command to the Gripper - the method takes a list of uint8 as an argument. The meaning of each variable depends on the Gripper model (see support.robotiq.com for more details)""" - #make sure data has an even number of elements + #make sure data has an even number of elements if(len(data) % 2 == 1): data.append(0) @@ -73,17 +75,17 @@ def sendCommand(self, data): message = [] #Fill message by combining two bytes in one register - for i in range(0, len(data)/2): + for i in range(0, len(data)//2): message.append((data[2*i] << 8) + data[2*i+1]) - #To do!: Implement try/except + #To do!: Implement try/except self.client.write_registers(0x03E8, message, unit=0x0009) def getStatus(self, numBytes): """Sends a request to read, wait for the response and returns the Gripper status. The method gets the number of bytes to read as an argument""" numRegs = int(ceil(numBytes/2.0)) - #To do!: Implement try/except + #To do!: Implement try/except #Get status from the device response = self.client.read_holding_registers(0x07D0, numRegs, unit=0x0009) @@ -94,6 +96,6 @@ def getStatus(self, numBytes): for i in range(0, numRegs): output.append((response.getRegister(i) & 0xFF00) >> 8) output.append( response.getRegister(i) & 0x00FF) - + #Output the result return output diff --git a/robotiq_modbus_tcp/package.xml b/robotiq_modbus_tcp/package.xml index 9c1c612a..d96fba24 100644 --- a/robotiq_modbus_tcp/package.xml +++ b/robotiq_modbus_tcp/package.xml @@ -10,6 +10,6 @@ catkin - python-pymodbus + python3-pymodbus rospy diff --git a/robotiq_modbus_tcp/src/robotiq_modbus_tcp/comModbusTcp.py b/robotiq_modbus_tcp/src/robotiq_modbus_tcp/comModbusTcp.py index ee416584..b23774cd 100644 --- a/robotiq_modbus_tcp/src/robotiq_modbus_tcp/comModbusTcp.py +++ b/robotiq_modbus_tcp/src/robotiq_modbus_tcp/comModbusTcp.py @@ -36,7 +36,7 @@ """@package docstring -Module comModbusTcp: defines a class which communicates with Robotiq Grippers using the Modbus TCP protocol. +Module comModbusTcp: defines a class which communicates with Robotiq Grippers using the Modbus TCP protocol. The module depends on pymodbus (http://code.google.com/p/pymodbus/) for the Modbus TCP client. """ @@ -52,7 +52,7 @@ class communication: def __init__(self): self.client = None self.lock = threading.Lock() - + def connectToDevice(self, address): """Connection to the client - the method takes the IP address (as a string, e.g. '192.168.1.11') as an argument.""" self.client = ModbusTcpClient(address) @@ -61,9 +61,9 @@ def disconnectFromDevice(self): """Close connection""" self.client.close() - def sendCommand(self, data): + def sendCommand(self, data): """Send a command to the Gripper - the method takes a list of uint8 as an argument. The meaning of each variable depends on the Gripper model (see support.robotiq.com for more details)""" - #make sure data has an even number of elements + #make sure data has an even number of elements if(len(data) % 2 == 1): data.append(0) @@ -71,7 +71,7 @@ def sendCommand(self, data): message = [] #Fill message by combining two bytes in one register - for i in range(0, len(data)/2): + for i in range(0, len(data)//2): message.append((data[2*i] << 8) + data[2*i+1]) #To do!: Implement try/except @@ -82,7 +82,7 @@ def getStatus(self, numBytes): """Sends a request to read, wait for the response and returns the Gripper status. The method gets the number of bytes to read as an argument""" numRegs = int(ceil(numBytes/2.0)) - #To do!: Implement try/except + #To do!: Implement try/except #Get status from the device with self.lock: response = self.client.read_input_registers(0, numRegs) @@ -94,6 +94,6 @@ def getStatus(self, numBytes): for i in range(0, numRegs): output.append((response.getRegister(i) & 0xFF00) >> 8) output.append( response.getRegister(i) & 0x00FF) - + #Output the result return output