Add RTU Modbus interface

robotiq_control/launch/cmodel_action_controller.launch

@@ -1,20 +1,28 @@
 <?xml version="1.0"?>
 <launch>
   <!-- launch file parameters -->
+
+  <!-- If false, RTU is used -->
+  <arg name="use_tcp_control_mode" default="true" />  
+
   <arg name="ip"		default="192.168.0.13" />
+  <arg name="port" default="" />
+  <arg name="device_name"		default="/tmp/ttyUR_a" />
   <arg name="namespace"		default="gripper" />
   <arg name="gripper_prefix"	default="" />
   <arg name="config"		default="cmodel_action_controller" />
 
   <group ns="$(arg namespace)">
     <!-- CModel TCP Controller -->
-    <node name="cmodel_tcp_driver"
-	  pkg="robotiq_control" type="cmodel_tcp_driver.py"
-	  output="screen" args="$(arg ip)"/>
+    <group if="$(arg use_tcp_control_mode)">
+      <node name="cmodel_tcp_driver" pkg="robotiq_control" type="cmodel_tcp_driver.py" output="screen" args="$(arg ip) $(arg port)" respawn="true"/>
+    </group>
+    <group unless="$(arg use_tcp_control_mode)">
+      <node name="cmodel_rtu_driver" pkg="robotiq_control" type="cmodel_rtu_driver.py" output="screen" args="$(arg device_name)" respawn="true"/>
+    </group>
     <param name="gripper_prefix"  type="str" value="$(arg gripper_prefix)"/>
     <!-- Simple action controller -->
-    <rosparam file="$(find robotiq_control)/config/$(arg config).yaml"
-	      command="load" />
+    <rosparam file="$(find robotiq_control)/config/$(arg config).yaml" command="load" />
     <node name="cmodel_action_controller" pkg="robotiq_control"
 	  type="cmodel_action_controller.py" output="screen" />
   </group>

robotiq_control/scripts/cmodel_rtu_driver.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+import os
+import sys
+import socket
+import rospy
+from robotiq_control.cmodel_base import RobotiqCModel, ComModbusRtu
+from robotiq_msgs.msg import CModelCommand, CModelStatus
+
+def mainLoop(device):
+  # Gripper is a C-Model with a TCP connection
+  gripper = RobotiqCModel()
+  gripper.client = ComModbusRtu()
+  # We connect to the device name received as an argument
+  rospy.loginfo("Connecting to device " + str(device))
+  gripper.client.connectToDevice(device)
+  # The Gripper status
+  pub = rospy.Publisher('status', CModelStatus, queue_size=3)
+  # The Gripper command
+  rospy.Subscriber('command', CModelCommand, gripper.refreshCommand)
+
+  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__':
+  rospy.init_node('cmodel_rtu_driver')
+  # Run the main loop
+  try:
+    mainLoop(sys.argv[1])
+  except rospy.ROSInterruptException: pass

robotiq_control/src/robotiq_control/cmodel_base.py

@@ -3,7 +3,7 @@
 import time
 import threading
 from robotiq_msgs.msg import CModelStatus
-from pymodbus.client.sync import ModbusTcpClient
+from pymodbus.client.sync import ModbusTcpClient, ModbusSerialClient
 from pymodbus.register_read_message import ReadInputRegistersResponse
 
 
@@ -59,6 +59,70 @@ def getStatus(self, numBytes):
     return output
 
 
+class ComModbusRtu:	
+
+   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
+          return False
+      return True
+
+   def disconnectFromDevice(self):
+      """Close connection"""
+      self.client.close()
+
+   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   
+      if(len(data) % 2 == 1):
+         data.append(0)
+
+      #Initiate message as an empty list
+      message = []
+
+      #Fill message by combining two bytes in one register
+      for i in range(0, len(data)/2):
+         message.append((data[2*i] << 8) + data[2*i+1])
+
+      try:
+         self.client.write_registers(0x03E8, message, unit=0x0009)
+      except Exception as e:
+         print("Encountered an exception when writing registers:")
+         print(e)
+
+
+   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))
+
+      #Get status from the device
+      try:
+         response = self.client.read_holding_registers(0x07D0, numRegs, unit=0x0009)
+      except Exception as e:
+         print("Encountered an exception when writing registers:")
+         print(e)
+
+      #Instantiate output as an empty list
+      output = []
+
+      #Fill the output with the bytes in the appropriate order
+      try:
+        for i in range(0, numRegs):
+          output.append((response.getRegister(i) & 0xFF00) >> 8)
+          output.append( response.getRegister(i) & 0x00FF)
+      except Exception as e:
+         print("Encountered an exception when writing registers:")
+         print(e)
+
+      #Output the result
+      return output
+
+
 class RobotiqCModel:
   def __init__(self):
     #Initiate output message as an empty list