Merge pull request #8 from marshallpowell97/master.

Author: gavanderhoorn

.gitignore

@@ -0,0 +1 @@
+staubli_val3_driver/val3/ros_server/.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 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/CS8C 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,8 @@ Refer to the `staubli_val3_driver` [readme](./staubli_val3_driver/README.md) for
 
 ## Compatibility
 
-The current version of the driver is compatible with CS8 controllers only.
-Future work may extend this to support CS9 controllers as well.
+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.
 
 
 

staubli_val3_driver/README.md

@@ -2,20 +2,19 @@
 
 ## Overview
 
-This ROS-I driver was developed in Staubli's VAL3 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 Staubli's emulator first.
+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.
 
 
 ## Requirements
 
 * Staubli 6-axis robot manipulator
-* Staubli CS8 controller
-  * it may work with other controllers, but this driver was tested on CS8 only
+* Staubli CS8/CS8C controller
 * VAL3 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
 
@@ -29,20 +28,22 @@ Clone [staubli_val3_driver](https://github.com/ros-industrial/staubli_val3_drive
 git clone https://github.com/ros-industrial/staubli_val3_driver.git
 ```
 
-### Transfer driver to Staubli CS8 controller
+### Transfer driver to Staubli CS8/CS8C controller
+
+There are 2 ways of transferring the VAL 3 application to the controller:
 
-There are several ways of transferring VAL3 applications to a Staubli controller.
-The simplest method is probably copying the contents of `val3` folder into a USB memory stick (<2GB given the CS8 limitations), plugging the stick to the controller and using the teach pendant to copy the folders.
+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. Use the Transfer Manager in SRS to copy the VAL 3 applications to the controller. (Home -> Controller -> Transfer Manager)
 
 ### Open the VAL3 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 the Staubli Robotics Suite:
+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/):
 
 * 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
 
-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
 
@@ -55,7 +56,7 @@ From `Main menu`:
 
 ### Configuration
 
-The TCP sockets on the CS8 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.
+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.
 
 Two sockets (TCP Servers) are required. From `Main menu`:
 
@@ -73,17 +74,17 @@ have been transferred to the Staubli controller
 2. The VAL3 application `ros_server` has been loaded
 3. Both TCP Server sockets have been configured properly
 
-Then simply press the `Run` button, ensure that `ros_server` is highlighted, then press `F8` (Ok).
+Press the `Run` button, ensure that `ros_server` is highlighted, then press `F8` (Ok).
 
-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).
+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).
 
 ### 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:
 
 ```shell
-roslaunch staubli_val3_driver robot_interface_streaming.launch robot_ip:=<CS8 controller IP address>
+roslaunch staubli_val3_driver robot_interface_streaming.launch robot_ip:=<controller IP address>
 ```
 
 ## Bugs, suggestions and feature requests

staubli_val3_driver/val3/ros_server/dataReceiver.pgx

@@ -29,9 +29,19 @@
   // 3: decode body
   // 4: push motion into buffer
   // 5: send ACK
-  l_nMsgRecvState=0
+  
+  // Set variable constants to make sure they were not changed
+  nReset=0
+  nRecHeader=0
+  nDecHeader=1
+  nRecBody=2
+  nDecBody=3
+  nPushMotion=4
+  nAck=5
+  
+  l_nMsgRecvState=nRecHeader
 
-  while (true)
+  while(true)
     // usually for state machines a switch-case statement is a neat implementation
     // HOWEVER, at every iteration of while() this task is sequenced and only called
     // after whatever ms is was defined when created.
@@ -41,30 +51,30 @@
     // into buffer and then send ACK to client WITHIN ONE ITERATION of while(),
     // hence the if statement sequence below
     l_nStartTime = clock()
-    if (l_nMsgRecvState==0)
+    if(l_nMsgRecvState==nRecHeader)
       call recvMsgHeader(l_nMsgRecvState)
     endIf
     nHeaderTime = clock() - l_nStartTime
-    if (l_nMsgRecvState==1)
+    if(l_nMsgRecvState==nDecHeader)
       call decodeMsgHeader(l_nMsgRecvState)
     endIf
-    if (l_nMsgRecvState==2)
+    if(l_nMsgRecvState==nRecBody)
       call recvMsgBody(l_nMsgRecvState)
     endIf
-    if (l_nMsgRecvState==3)
+    if(l_nMsgRecvState==nDecBody)
       call decodeMsgBody(l_nMsgRecvState)
     endIf
-    if (l_nMsgRecvState==4)
+    if(l_nMsgRecvState==nPushMotion)
       call pushMotion(l_nMsgRecvState)
     endIf
-    if (l_nMsgRecvState==5)
+    if(l_nMsgRecvState==nAck)
       call encodeAck()
       call sendAck(l_nMsgRecvState)
     endIf
     nElapsedTime = clock() - l_nStartTime - nHeaderTime
     
     // assert state machine is working OK
-    if (l_nMsgRecvState<0 or l_nMsgRecvState>5)
+    if(l_nMsgRecvState<nRecHeader or l_nMsgRecvState>nAck)
       popUpMsg("Data reception state machine error")
     endIf
 
@@ -75,4 +85,4 @@
   endWhile
 end]]></Code>
   </Program>
-</Programs>
+</Programs>

staubli_val3_driver/val3/ros_server/decTrajPt.pgx

@@ -1,8 +1,8 @@
-<?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="decTrajPt" access="public" >
-    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1" >
-      <Parameter name="x_bFlag" type="bool" use="reference" xsi:type="element" dimensions="1" />
+<?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="decTrajPt" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_bFlag" type="bool" xsi:type="element" use="reference" />
     </Parameters>
     <Locals>
       <Local name="l_nIndex" type="num" xsi:type="array" size="1" />
@@ -32,30 +32,30 @@
 
   // sequence
   l_nRetVal=fromBinary(rosTrajPtMsg.body.nData,4,"-4l",rosTrajPtMsg.jointTrajPt.nSeq)
-  if (l_nRetVal!=1)
+  if(l_nRetVal!=1)
     x_bFlag=false
     return
   endIf
 
   // joints[10]
   for l_nIndex=0 to 9 step 1
     l_nRetVal=fromBinary(rosTrajPtMsg.body.nData[(l_nIndex*4)+4],4,"4.0l",rosTrajPtMsg.jointTrajPt.nJoints[l_nIndex])
-    if (l_nRetVal!=1)
+    if(l_nRetVal!=1)
       x_bFlag=false
       return
     endIf
   endFor
 
   // velocity
   l_nRetVal=fromBinary(rosTrajPtMsg.body.nData[44],4,"4.0l",rosTrajPtMsg.jointTrajPt.nVelocity)
-  if (l_nRetVal!=1)
+  if(l_nRetVal!=1)
     x_bFlag=false
     return
   endIf
 
   // duration
   l_nRetVal=fromBinary(rosTrajPtMsg.body.nData[48],4,"4.0l",rosTrajPtMsg.jointTrajPt.nDuration)
-  if (l_nRetVal!=1)
+  if(l_nRetVal!=1)
     x_bFlag=false
     return
   endIf
@@ -66,4 +66,4 @@
   x_bFlag=true
 end]]></Code>
   </Program>
-</Programs>
+</Programs>

staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx

@@ -1,8 +1,8 @@
-<?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="decTrajPtFull" access="public" >
-    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1" >
-      <Parameter name="x_bFlag" type="bool" use="reference" xsi:type="element" dimensions="1" />
+<?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="decTrajPtFull" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_bFlag" type="bool" xsi:type="element" use="reference" />
     </Parameters>
     <Locals>
       <Local name="l_nIndex" type="num" xsi:type="array" size="1" />
@@ -32,36 +32,36 @@
 
   // robotId
   l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData,4,"-4l",rosTrajPtFMsg.jointTrajPtFull.nRobotId)
-  if (l_nRetVal!=1)
+  if(l_nRetVal!=1)
     x_bFlag=false
     return
   endIf
 
   // sequence
   l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData,4,"-4l",rosTrajPtFMsg.jointTrajPtFull.nSeq)
-  if (l_nRetVal!=1)
+  if(l_nRetVal!=1)
     x_bFlag=false
     return
   endIf
 
   // valid fields
   l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData,4,"-4l",rosTrajPtFMsg.jointTrajPtFull.nValidFields)
-  if (l_nRetVal!=1)
+  if(l_nRetVal!=1)
     x_bFlag=false
     return
   endIf
 
   // time
   l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData,4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nTime)
-  if (l_nRetVal!=1)
+  if(l_nRetVal!=1)
     x_bFlag=false
     return
   endIf
 
   // positions[10]
   for l_nIndex=0 to 9 step 1
     l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData[(l_nIndex*4)+16],4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nPositions[l_nIndex])
-    if (l_nRetVal!=1)
+    if(l_nRetVal!=1)
       x_bFlag=false
       return
     endIf
@@ -70,7 +70,7 @@
   // velocities[10]
   for l_nIndex=0 to 9 step 1
     l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData[(l_nIndex*4)+56],4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nVelocities[l_nIndex])
-    if (l_nRetVal!=1)
+    if(l_nRetVal!=1)
       x_bFlag=false
       return
     endIf
@@ -79,7 +79,7 @@
   // accelerations[10]
   for l_nIndex=0 to 9 step 1
     l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData[(l_nIndex*4)+96],4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nAccelerations[l_nIndex])
-    if (l_nRetVal!=1)
+    if(l_nRetVal!=1)
       x_bFlag=false
       return
     endIf
@@ -91,4 +91,4 @@
   x_bFlag=true
 end]]></Code>
   </Program>
-</Programs>
+</Programs>

staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx

@@ -27,10 +27,10 @@
   l_bFlag=false
 
   switch rosGenericMsg.header.nMsgType
-    case 11
+    case nStdMsgType["JOINT_TRAJ_PT"]
       call decTrajPt(l_bFlag)
     break
-    case 14
+    case nStdMsgType["JOINT_TRAJ_PT_FULL"]
       call decTrajPtFull(l_bFlag)
     break
     default
@@ -39,27 +39,27 @@
 
   if (l_bFlag == true)
     // move on to state 4: push motion into buffer
-    x_nMsgRecvState=4
+    x_nMsgRecvState=nPushMotion
   else
     // error whilst decoding msg body...
     if (rosGenericMsg.header.nCommType==2)
       switch rosGenericMsg.header.nMsgType
-        case 11
+        case nStdMsgType["JOINT_TRAJ_PT"]
           rosTrajPtAck.header.nReplyCode=2
         break
-        case 14
+        case nStdMsgType["JOINT_TRAJ_PT_FULL"]
           rosTrajPtFAck.header.nReplyCode=2
         break
         default
         break
       endSwitch
       // jump to state 5: sendAck()
-      x_nMsgRecvState=5
+      x_nMsgRecvState=nAck
     else
       // reset state machine, since no ACK is required
-      x_nMsgRecvState=0
+      x_nMsgRecvState=nReset
     endIf
   endIf
 end]]></Code>
   </Program>
-</Programs>
+</Programs>

staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx

@@ -28,31 +28,31 @@
   l_nRetVal=fromBinary(rosGenericMsg.prefix.nData,4,"-4l",rosGenericMsg.prefix.nLength)
   if (l_nRetVal!=1)
     // error decoding prefix, reset state machine
-    x_nMsgRecvState=0
+    x_nMsgRecvState=nReset
     return
   endIf
   l_nRetVal=fromBinary(rosGenericMsg.header.nData,4,"-4l",rosGenericMsg.header.nMsgType)
   if (l_nRetVal!=1)
     // error decoding header (msg type), reset state machine
-    x_nMsgRecvState=0
+    x_nMsgRecvState=nReset
     return
   endIf
   l_nRetVal=fromBinary(rosGenericMsg.header.nData[4],4,"-4l",rosGenericMsg.header.nCommType)
   if (l_nRetVal!=1)
     // error decoding header (comm type), reset state machine
-    x_nMsgRecvState=0
+    x_nMsgRecvState=nReset
     return
   endIf
   l_nRetVal=fromBinary(rosGenericMsg.header.nData[8],4,"-4l",rosGenericMsg.header.nReplyCode)
   if (l_nRetVal!=1)
     // error decoding header (reply code), reset state machine
-    x_nMsgRecvState=0
+    x_nMsgRecvState=nReset
     return
   endIf
 
   // no errors detected whilst decoding prefix and header
   // move on to state 2: receive msg body
-  x_nMsgRecvState=2
+  x_nMsgRecvState=nRecBody
 end]]></Code>
   </Program>
-</Programs>
+</Programs>