Merge pull request #15 from marshallpowell97/master

Add support for CS9 controller

.gitignore

@@ -1 +1,2 @@
 staubli_val3_driver/val3/ros_server/.outlining.json
+*outlining.json

README.md

@@ -9,7 +9,7 @@
 ## Overview
 
 This repository contains the `staubli_val3_driver` package which provides a set of VAL3 libraries and an application which together implement a [simple_message][] compatible server implementation.
-Together with the nodes in [industrial_robot_client][], this server can be used as a ROS 1 driver that allows motion control of Staubli CS8/CS8C controlled robots, by exposing a [FollowJointTrajectory][] [action][] server, which is compatible with MoveIt and other nodes that implement an action client.
+Together with the nodes in [industrial_robot_client][], this server can be used as a ROS 1 driver that allows motion control of Staubli CS8/CS9 controlled robots, by exposing a [FollowJointTrajectory][] [action][] server, which is compatible with MoveIt and other nodes that implement an action client.
 
 
 ## Documentation
@@ -19,8 +19,7 @@ Refer to the `staubli_val3_driver` [readme](./staubli_val3_driver/README.md) for
 
 ## Compatibility
 
-The current version of the driver is compatible with CS8/CS8C controllers only.
-Future work is planned to extend this to support CS9 controllers as well.
+The current version of the driver is compatible with Staubli CS8 and CS9 controllers.
 
 
 

staubli_val3_driver/README.md

@@ -2,48 +2,56 @@
 
 ## Overview
 
-This ROS-I driver was developed in Staubli's VAL 3 language for use with 6-axis Staubli robot manipulators.
+This ROS-I driver was developed in Staubli's VAL 3 language for use with 6-axis
+Staubli robot manipulators.
 
-It is advisable to try this driver on the emulator in [Staubli Robotics Suite (SRS)](https://www.staubli.com/en-us/robotics/product-range/robot-software/pc-robot-programming-srs/) first.
+It is advisable to try this driver on Staubli's emulator in Staubli Robotics Suite (SRS) first.
 
 
 ## Requirements
 
 * Staubli 6-axis robot manipulator
-* Staubli CS8/CS8C controller
-* VAL3 version s7.7.2 or greater
+* Staubli CS8/CS9 controller
+* VAL 3 version s7.7.2 or greater
   * this is very important, since this implementation uses return values of `sioGet()`
     only available from s7.7.2 onwards
 * Staubli Robotics Suite 2019 (not required but strongly recommended)
 
 ## Installation
 
-Installing the driver to a Staubli controller simply consists of transferring the contents of the `val3` folder to the controller itself.
+Installing the driver to a Staubli controller simply consists of transferring the
+contents of the `val3` folder to the controller itself.
 
 ### Clone this repository
 
-Clone [staubli_val3_driver](https://github.com/ros-industrial/staubli_val3_driver) into the `src` space of your workspace:
+Clone branch `kinetic-devel` of [staubli_experimental](https://github.com/ros-industrial/staubli_experimental):
 
 ```shell
-git clone https://github.com/ros-industrial/staubli_val3_driver.git
+git clone https://github.com/ros-industrial/staubli_experimental -b kinetic-devel
 ```
 
-### Transfer driver to Staubli CS8/CS8C controller
+### Transfer driver to Staubli controller
 
-There are 2 ways of transferring the VAL 3 application to the controller:
+There are multiple ways of transferring VAL 3 applications to the controller:
 
-1. Copy the contents of `val3` folder onto a USB memory stick (<2GB given the CS8/CS8C limitations), plugging the stick into the controller and using the teach pendant to copy the folders
+1. Copy the contents of `val3` folder onto a USB memory stick (<2GB if using CS8), 
+plugging the stick into the controller and using the teach pendant to copy the folders
 
-1. Use the Transfer Manager in SRS to copy the VAL 3 applications to the controller. (Home -> Controller -> Transfer Manager)
+2. Use the Transfer Manager in SRS to copy the contents of `val3` folder to the controller. (Home -> Controller -> Transfer Manager)
 
-### Open the VAL3 application with Staubli SRS
+2. Use an FTP software to copy the contents of `val3` folder to the controller.
 
-Although it is possible to edit the source files with any text editor (they are essentially XML files), it is advisable to use [Staubli Robotics Suite](https://www.staubli.com/en-us/robotics/product-range/robot-software/pc-robot-programming-srs/):
+### Open the VAL 3 application with Staubli SRS
+
+Although it is possible to edit the source files with any text editor (they are
+essentially XML files), it is advisable to use Staubli Robotics Suite:
 
 * Copy contents of folder `val3` into the `usrapp` folder of the Staubli cell
-* Open the `ros_server` VAL3 project located inside the `ros_server` folder
+* Open the `ros_server` VAL 3 appplication located inside the `ros_server` folder
+
+SRS offers autocompletion, syntax highlighting and syntax checking, amongst other
+useful features, such as a Staubli controller/teach pendant emulator.
 
-SRS offers autocompletion, syntax highlighting and syntax checking, amongst other useful features, such as a Staubli controller/teach pendant emulator. 
 
 ## Usage
 
@@ -56,37 +64,63 @@ From `Main menu`:
 
 ### Configuration
 
-The TCP sockets on the CS8/CS8C controller/emulator must be configured prior to use, otherwise a runtime error will be generated and the driver will not work.
+The TCP sockets on the CS8/CS9 controller/emulator must be configured prior to using
+the driver, otherwise a runtime error will be displayed on the teach pendant and
+the driver will not work.
+
+Two sockets (TCP Servers) are required.
 
-Two sockets (TCP Servers) are required. From `Main menu`:
+#### CS8
+
+ From `Main menu`:
 
 1. Control panel --> I/O --> Socket --> TCP Servers
 2. Configure two sockets
-   * Name: Feedback, Port: 11002, Timeout: -1, Delimiter: 13, Nagle: Off
-   * Name: Motion, Port: 11000, Timeout: -1, Delimiter: 13, Nagle: Off
+   * Name: Feedback, Port: 11002, Timeout: -1, End of string: 13, Nagle: Off
+   * Name: Motion, Port: 11000, Timeout: -1, End of string: 13, Nagle: Off
+
+#### CS9
+
+ From `Home`:
+
+1. IO --> Socket --> TCP Servers --> "+"
+2. Configure two sockets
+   * Name: Feedback, Port: 11002, Timeout: -1, End of string: 13, Nagle: Off
+   * Name: Motion, Port: 11000, Timeout: -1, End of string: 13, Nagle: Off
 
 ### Run the driver (ROS-I server)
 
 Check that:
 
 1. The contents of the `val3` folder (both `ros_server` and `ros_libs` folders)
 have been transferred to the Staubli controller
-2. The VAL3 application `ros_server` has been loaded
+2. The VAL 3 application `ros_server` has been loaded
 3. Both TCP Server sockets have been configured properly
 
-Press the `Run` button, ensure that `ros_server` is highlighted, then press `F8` (Ok).
+#### CS8
+
+Press the `Run` button, ensure that `ros_server` is highlighted,
+then press `F8` (Ok).
+
+#### CS9
+
+VAL# --> Memory --> select `ros_server` --> ▶
 
-Depending on which working mode the controller is in, arm power may need to be enabled manually (a message will pop up on the screen).
-Once arm power is enabled, the robot will only move if the `Move` button has been pressed (or is kept pressed if in manual mode).
+Notice that depending on which mode of operation is currently active, the motors
+may need to be enabled manually (a message will pop up on the screen). Likewise,
+the robot will only move if the `Move` button has been pressed (or is kept pressed
+if in manual mode).
 
 ### Run the industrial_robot_client node (ROS-I client)
 
-The `indigo-devel` branch provides launch files (within the `staubli_val3_driver` ROS package). Simply run:
+The `kinetic-devel` branch provides launch files (within the `staubli_val3_driver`
+ROS package). Simply run:
 
 ```shell
-roslaunch staubli_val3_driver robot_interface_streaming.launch robot_ip:=<controller IP address>
+roslaunch staubli_val3_driver robot_interface_streaming.launch robot_ip:=<Controller IP address>
 ```
 
 ## Bugs, suggestions and feature requests
 
-Please report any bugs you may find, make any suggestions you may have and request new features you may find useful via GitHub.
+Please report any bugs you may find, make any suggestions you may have and request
+new features you may find useful via GitHub.

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/ack.pgx

@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="ack" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nHeaderTime" type="num" xsi:type="element" />
+      <Parameter name="x_nElapsedTime" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // display time between valid trajectory point received and ACK sent
+  
+  gotoxy(scMonitor, 0,9)
+  put(scMonitor, "recvMsgHeader:")
+  gotoxy(scMonitor, 15,9)
+  put(scMonitor, toString(".4", x_nHeaderTime))
+    
+  gotoxy(scMonitor, 0,10)
+  put(scMonitor, "other states:")
+  gotoxy(scMonitor, 15,10)
+  put(scMonitor, toString(".4", x_nElapsedTime))
+  
+end]]></Code>
+  </Program>
+</Programs>

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/clearScreen.pgx

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="clearScreen" access="public">
+    <Code><![CDATA[begin
+  
+  // Clear pendant screen
+  
+  cls(scMonitor)
+  
+end]]></Code>
+  </Program>
+</Programs>

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/debug.pgx

@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="debug" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nExecStatus" type="num" xsi:type="element" use="reference" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Debug
+  
+  gotoxy(scMonitor, 18,12)
+  switch x_nExecStatus
+    case 0
+      put(scMonitor, "[+]")
+      x_nExecStatus = 1
+    break
+    case 1
+      put(scMonitor, "[-]")
+      x_nExecStatus = 0
+    break
+    default
+    break
+  endSwitch
+  
+end]]></Code>
+  </Program>
+</Programs>

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/feedbackStatus.pgx

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="feedbackStatus" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nOutConnFlag" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update connection status on port 11002 (feedback server)
+  
+  gotoxy(scMonitor, 0,1)
+  // 11 characteres: gotoxy(12,1) to insert status
+  put(scMonitor, "Port 11002:")
+  gotoxy(scMonitor, 12,1)
+  switch x_nOutConnFlag
+    case -1
+      put(scMonitor, "connection lost")
+    break
+    case 0
+      put(scMonitor, "not connected")
+    break
+    case 1
+      put(scMonitor, "connected")
+    break
+    default
+      put(scMonitor, "not connected")
+    break
+  endSwitch
+  
+end]]></Code>
+  </Program>
+</Programs>

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/init.pgx

@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="init" access="public">
+    <Code><![CDATA[begin
+  
+  // Init user page
+  
+  userPage(scMonitor)
+  cls(scMonitor)
+  title(scMonitor, "ROS-I server")
+  
+end]]></Code>
+  </Program>
+</Programs>

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/interfaceCS8.dtx

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Database xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Data/2">
+  <Datas>
+    <Data name="mNomSpeed" access="private" xsi:type="array" type="mdesc" size="1" />
+    <Data name="scMonitor" access="private" xsi:type="array" type="screen" size="1" />
+  </Datas>
+</Database>

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/interfaceCS8.pjx

@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project xmlns="http://www.staubli.com/robotics/VAL3/Project/3">
+  <Parameters version="s7.11" stackSize="5000" millimeterUnit="true" />
+  <Programs>
+    <Program file="ack.pgx" />
+    <Program file="clearScreen.pgx" />
+    <Program file="debug.pgx" />
+    <Program file="feedbackStatus.pgx" />
+    <Program file="init.pgx" />
+    <Program file="motionBuffer.pgx" />
+    <Program file="motionProgress.pgx" />
+    <Program file="motionStatus.pgx" />
+    <Program file="start.pgx" />
+    <Program file="stop.pgx" />
+    <Program file="trajBuffer.pgx" />
+    <Program file="velOverwrite.pgx" />
+  </Programs>
+  <Database>
+    <Data file="interfaceCS8.dtx" />
+  </Database>
+  <Libraries>
+    <Library alias="libQueueFuncs" path="Disk://ros_libs/QueueFuncs/QueueFuncs.pjx" />
+  </Libraries>
+</Project>

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionBuffer.pgx

@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionBuffer" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nMovePts" type="num" xsi:type="element" />
+      <Parameter name="x_nElems" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update motion buffer size
+  
+  gotoxy(scMonitor, 0,4)
+  put(scMonitor, "Motion buffer:")
+  gotoxy(scMonitor, 19,4)
+  put(scMonitor, x_nElems)
+  gotoxy(scMonitor, 24,4)
+  put(scMonitor, x_nMovePts)
+  
+end]]></Code>
+  </Program>
+</Programs>

staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionProgress.pgx

@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionProgress" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nMoveId" type="num" xsi:type="element" />
+      <Parameter name="x_nMotProg" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // display nMoveId and nMotionProgress
+  
+  gotoxy(scMonitor, 0,7)
+  put(scMonitor, "moveId:")
+  gotoxy(scMonitor, 8,7)
+  put(scMonitor, x_nMoveId)
+  gotoxy(scMonitor, 13,7)
+  put(scMonitor, "progress:")
+  gotoxy(scMonitor, 23,7)
+  put(scMonitor, x_nMotProg)
+  
+end]]></Code>
+  </Program>
+</Programs>