From eb7b66929b24141fd132b8d55aeba686a6ad3fbc Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Wed, 12 Jun 2019 17:57:40 +0200
Subject: [PATCH] added a node to startup RS485 interface

and added it to the launchfiles.
---
 .../launch/ur10e_ros_control.launch           |  6 ++-
 ur_rtde_driver/launch/ur3e_ros_control.launch |  6 ++-
 ur_rtde_driver/launch/ur5e_ros_control.launch |  6 ++-
 ur_rtde_driver/launch/ur_common.launch        | 12 +++++-
 ur_rtde_driver/scripts/tool_communication     | 39 +++++++++++++++++++
 5 files changed, 61 insertions(+), 8 deletions(-)
 create mode 100755 ur_rtde_driver/scripts/tool_communication

diff --git a/ur_rtde_driver/launch/ur10e_ros_control.launch b/ur_rtde_driver/launch/ur10e_ros_control.launch
index ce13461..b48c870 100644
--- a/ur_rtde_driver/launch/ur10e_ros_control.launch
+++ b/ur_rtde_driver/launch/ur10e_ros_control.launch
@@ -18,11 +18,13 @@
   <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
   <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
   <arg name="tool_voltage" default="12"/>
-  <arg name="tool_parity" default="1"/>
-  <arg name="tool_baud_rate" default="57600"/>
+  <arg name="tool_parity" default="0"/>
+  <arg name="tool_baud_rate" default="115200"/>
   <arg name="tool_stop_bits" default="1"/>
   <arg name="tool_rx_idle_chars" default="1.5"/>
   <arg name="tool_tx_idle_chars" default="3.5"/>
+  <arg name="tool_device_name" default="/tmp/ttyUR"/>
+  <arg name="tool_tcp_port" default="54321"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
diff --git a/ur_rtde_driver/launch/ur3e_ros_control.launch b/ur_rtde_driver/launch/ur3e_ros_control.launch
index c29bd17..5f535b4 100644
--- a/ur_rtde_driver/launch/ur3e_ros_control.launch
+++ b/ur_rtde_driver/launch/ur3e_ros_control.launch
@@ -18,11 +18,13 @@
   <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
   <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
   <arg name="tool_voltage" default="12"/>
-  <arg name="tool_parity" default="1"/>
-  <arg name="tool_baud_rate" default="57600"/>
+  <arg name="tool_parity" default="0"/>
+  <arg name="tool_baud_rate" default="115200"/>
   <arg name="tool_stop_bits" default="1"/>
   <arg name="tool_rx_idle_chars" default="1.5"/>
   <arg name="tool_tx_idle_chars" default="3.5"/>
+  <arg name="tool_device_name" default="/tmp/ttyUR"/>
+  <arg name="tool_tcp_port" default="54321"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
diff --git a/ur_rtde_driver/launch/ur5e_ros_control.launch b/ur_rtde_driver/launch/ur5e_ros_control.launch
index 1aab976..edb8451 100644
--- a/ur_rtde_driver/launch/ur5e_ros_control.launch
+++ b/ur_rtde_driver/launch/ur5e_ros_control.launch
@@ -18,11 +18,13 @@
   <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
   <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
   <arg name="tool_voltage" default="12"/>
-  <arg name="tool_parity" default="1"/>
-  <arg name="tool_baud_rate" default="57600"/>
+  <arg name="tool_parity" default="0"/>
+  <arg name="tool_baud_rate" default="115200"/>
   <arg name="tool_stop_bits" default="1"/>
   <arg name="tool_rx_idle_chars" default="1.5"/>
   <arg name="tool_tx_idle_chars" default="3.5"/>
+  <arg name="tool_device_name" default="/tmp/ttyUR"/>
+  <arg name="tool_tcp_port" default="54321"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
diff --git a/ur_rtde_driver/launch/ur_common.launch b/ur_rtde_driver/launch/ur_common.launch
index e42c395..87becf8 100644
--- a/ur_rtde_driver/launch/ur_common.launch
+++ b/ur_rtde_driver/launch/ur_common.launch
@@ -18,11 +18,13 @@
   <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
   <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
   <arg name="tool_voltage" default="12"/>
-  <arg name="tool_parity" default="1"/>
-  <arg name="tool_baud_rate" default="57600"/>
+  <arg name="tool_parity" default="0"/>
+  <arg name="tool_baud_rate" default="115200"/>
   <arg name="tool_stop_bits" default="1"/>
   <arg name="tool_rx_idle_chars" default="1.5"/>
   <arg name="tool_tx_idle_chars" default="3.5"/>
+  <arg name="tool_device_name" default="/tmp/ttyUR"/>
+  <arg name="tool_tcp_port" default="54321"/>
 
   <!-- robot model -->
   <include file="$(arg robot_description_file)">
@@ -45,6 +47,12 @@
     <param name="tool_tx_idle_chars" value="$(arg tool_tx_idle_chars)"/>
   </node>
 
+  <node if="$(arg use_tool_communication)" name="ur_tool_communication_bridge" pkg="ur_rtde_driver" type="tool_communication" respawn="false" output="screen">
+    <param name="robot_ip" value="$(arg robot_ip)"/>
+    <param name="device_name" value="$(arg tool_device_name)"/>
+    <param name="tool_tcp_port" value="$(arg tool_tcp_port)"/>
+  </node>
+
   <!-- Load controller settings -->
   <rosparam file="$(arg controller_config_file)" command="load"/>
 
diff --git a/ur_rtde_driver/scripts/tool_communication b/ur_rtde_driver/scripts/tool_communication
new file mode 100755
index 0000000..7bf4410
--- /dev/null
+++ b/ur_rtde_driver/scripts/tool_communication
@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+
+import os
+import subprocess
+import rospy
+
+
+def main():
+    """Starts socat"""
+    rospy.init_node("ur_tool_communication")
+
+    # IP address of the robot
+    robot_ip = rospy.get_param("~robot_ip")
+    # Port on which the remote pc (robot) publishes the interface
+    tcp_port = rospy.get_param("tcp_port", "54321")
+
+    # By default, socat will create a pty in /dev/pts/N with n being an increasing number.
+    # Additionally, a symlink at the given location will be created. Use an absolute path here.
+    local_device = rospy.get_param("~device_name")
+
+
+    rospy.loginfo("Remote device is available at '" + local_device + "'")
+
+    cfg_params = ["pty"]
+    cfg_params.append("link=" + local_device)
+    cfg_params.append("raw")
+    cfg_params.append("ignoreeof")
+    cfg_params.append("waitslave")
+
+    cmd = ["socat"]
+    cmd.append(",".join(cfg_params))
+    cmd.append(":".join(["tcp", robot_ip, tcp_port]))
+
+    rospy.loginfo("Starting socat with following command:\n" + " ".join(cmd))
+    subprocess.call(cmd)
+
+
+if __name__ == '__main__':
+    main()