Connect gripper+FT via Bluetooth

robotiq_3f_gripper_control/nodes/Robotiq3FGripperRtuNode.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+
+"""@package docstring
+ROS node for controling a Robotiq S-Model gripper using the Modbus RTU protocol.
+
+The script takes as an argument the device address of the gripper (e.g. /dev/ttyUSB0). Afterwards it acts like the
+Robotiq3FGripperTcpNode.
+"""
+
+import roslib; roslib.load_manifest('robotiq_3f_gripper_control')
+roslib.load_manifest('robotiq_modbus_tcp')
+import rospy
+import robotiq_3f_gripper_control.baseRobotiq3FGripper
+import robotiq_modbus_rtu.comModbusRtu
+import os, sys
+from robotiq_3f_gripper_control.msg import _Robotiq3FGripper_robot_input  as inputMsg
+from robotiq_3f_gripper_control.msg import _Robotiq3FGripper_robot_output as outputMsg
+
+
+def mainLoop(device):
+    # Gripper is a 3F gripper with a TCP connection
+    gripper = robotiq_3f_gripper_control.baseRobotiq3FGripper.robotiqbaseRobotiq3FGripper()
+    gripper.client = robotiq_modbus_rtu.comModbusRtu.communication(retry=True)
+
+    # We connect to the address received as an argument
+    gripper.client.connectToDevice(device)
+
+    rospy.init_node('robotiq3FGripper')
+
+    # The Gripper status is published on the topic named 'Robotiq3FGripperRobotInput'
+    pub = rospy.Publisher('Robotiq3FGripperRobotInput', inputMsg.Robotiq3FGripper_robot_input, queue_size=1)
+
+    # The Gripper command is received from the topic named 'Robotiq3FGripperRobotOutput'
+    rospy.Subscriber('Robotiq3FGripperRobotOutput', outputMsg.Robotiq3FGripper_robot_output, gripper.refreshCommand)
+
+    # We loop
+    while not rospy.is_shutdown():
+        # Get and publish the Gripper status
+        status = gripper.getStatus()
+        pub.publish(status)
+
+        # Wait a little
+        rospy.sleep(0.05)
+
+        # Send the most recent command
+        gripper.sendCommand()
+
+        # Wait a little
+        rospy.sleep(0.05)
+
+
+if __name__ == '__main__':
+    try:
+        # TODO: Add verification that the argument is a valid device
+        mainLoop(sys.argv[1])
+    except rospy.ROSInterruptException:
+        pass

robotiq_ft_sensor/src/rq_sensor_com.cpp

@@ -170,7 +170,8 @@ INT_8 rq_sensor_com()
     {
         //Look for a serial device
         if (strstr(entrydirectory->d_name, "ttyS") ||
-            strstr(entrydirectory->d_name, "ttyUSB"))
+            strstr(entrydirectory->d_name, "ttyUSB") ||
+            strstr(entrydirectory->d_name, "rfcomm"))
         {
             device_found = rq_com_identify_device(entrydirectory->d_name);
         }
@@ -1182,6 +1183,7 @@ static UINT_8 rq_com_identify_device(INT_8 const * const d_name)
 	strcat(dirParent, d_name);
 	strcpy(port_com, dirParent);
 	fd_connexion = open(port_com, O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL);
+	usleep(1000000);
 
     //The serial port is open
     if (fd_connexion != -1)

robotiq_ft_sensor/src/rq_sensor_state.cpp

@@ -73,10 +73,16 @@ INT_8 rq_sensor_state(unsigned int max_retries, const std::string& ftdi_id)
 	switch (current_state)
 	{
 	case RQ_STATE_INIT:
+	    //first make sure there is no stream running
+	    rq_com_listen_stream();
+            if(rq_com_get_valid_stream() == true){
+                current_state = RQ_STATE_RUN;
+            }
+
 		ret = rq_state_init_com(ftdi_id);
 		if(ret == -1)
 		{
-            return -1;
+                    return -1;
 		}
 		break;
 

robotiq_modbus_rtu/src/robotiq_modbus_rtu/comModbusRtu.py

@@ -44,16 +44,20 @@
 """
 
 from pymodbus.client.sync import ModbusSerialClient
+from pymodbus.register_read_message import ReadHoldingRegistersResponse
+from pymodbus.register_write_message import WriteMultipleRegistersResponse
 from math import ceil
 
 class communication:	
 
-   def __init__(self):
+   def __init__(self, retry=False):
+      """Setting the retry parameter to True will lead to retries until an answer is delivered."""
       self.client = None
+      self.retry = retry
 
    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)
+      self.client = ModbusSerialClient(method='rtu',port=device,stopbits=1, bytesize=8, baudrate=115200, timeout=0.05)
       if not self.client.connect():
           print "Unable to connect to %s" % device
           return False
@@ -76,16 +80,30 @@ def sendCommand(self, data):
       for i in range(0, len(data)/2):
          message.append((data[2*i] << 8) + data[2*i+1])
 
-      #To do!: Implement try/except 
-      self.client.write_registers(0x03E8, message, unit=0x0009)
+      if self.retry:
+         while True:
+            response = self.client.write_registers(0x03E8, message, unit=0x0009)
+            if isinstance(response, WriteMultipleRegistersResponse):
+               break
+      else:
+         # 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 
       #Get status from the device
-      response = self.client.read_holding_registers(0x07D0, numRegs, unit=0x0009)
+      if self.retry:
+         while True:
+            response = self.client.read_holding_registers(0x07D0, numRegs, unit=0x0009)
+            if isinstance(response, ReadHoldingRegistersResponse):
+               break
+      else:
+         # To do!: Implement try/except
+         response = self.client.read_holding_registers(0x07D0, numRegs, unit=0x0009)
+         if not response:
+            print("Could not read registers on gripper. Please check connection and chosen device.")
 
       #Instantiate output as an empty list
       output = []