Add integration tests for automated testing

.github/dockerursim/.vol/default.variables

@@ -0,0 +1,2 @@
+#
+#Thu May 28 16:20:13 BST 2020

.github/dockerursim/.vol/programs.UR5

@@ -0,0 +1 @@
+/ursim/programs.UR5

.github/dockerursim/Dockerfile

@@ -0,0 +1,121 @@
+# MIT License
+#
+# Original from https://github.com/ahobsonsayers/DockURSim
+# Copyright (c) 2019 Arran Hobson Sayers
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+FROM lsiobase/guacgui:latest
+
+# Set Version Information
+ARG BUILD_DATE="15/08/19"
+ARG VERSION="5.8.0.10253"
+LABEL build_version="URSim Version: ${VERSION} Build Date: ${BUILD_DATE}"
+LABEL maintainer="Arran Hobson Sayers"
+LABEL MAINTAINER="Arran Hobson Sayers"
+ENV APPNAME="URSim"
+
+# Set Timezone
+ARG TZ="Europe/London"
+ENV TZ ${TZ}
+
+# Setup Environment
+ENV DEBIAN_FRONTEND noninteractive
+
+# Set Home Directory
+ENV HOME /ursim
+
+# Set robot model - Can be UR3, UR5 or UR10
+ENV ROBOT_MODEL UR5
+
+RUN \
+    echo "**** Installing Dependencies ****" && \
+    apt-get update && \
+    apt-get install -qy --no-install-recommends \
+    openjdk-8-jre psmisc && \
+    # Change java alternatives so we use openjdk8 (required by URSim) not openjdk11 that comes with guacgui
+    update-alternatives --install /usr/bin/java java /usr/lib/jvm/java-8-openjdk-amd64/jre/bin/java 10000
+
+# Setup JAVA_HOME
+ENV JAVA_HOME /usr/lib/jvm/java-8-openjdk-amd64
+
+RUN \
+    echo "**** Downloading URSim ****" && \
+    # Make sure we are in the root
+    cd / && \
+    # Download URSim Linux tar.gz
+    curl https://s3-eu-west-1.amazonaws.com/ur-support-site/69987/URSim_Linux-5.8.0.10253.tar.gz -o URSim-Linux.tar.gz && \
+    #curl https://s3-eu-west-1.amazonaws.com/ur-support-site/54411/URSim_Linux-5.4.2.76197.tar.gz -o URSim-Linux.tar.gz && \
+    # Extract tarball
+    tar xvzf URSim-Linux.tar.gz && \
+    #Remove the tarball
+    rm URSim-Linux.tar.gz && \
+    # Rename the URSim folder to jus ursim
+    mv  /ursim* /ursim
+
+RUN \
+    echo "**** Installing URSim ****" && \
+    # cd to ursim folder
+    cd /ursim && \
+    # Make URControl and all sh files executable
+    chmod +x ./*.sh ./URControl && \
+    #
+    # Stop install of unnecessary packages and install required ones quietly
+    sed -i 's|apt-get -y install|apt-get -qy install --no-install-recommends|g' ./install.sh && \
+    # Skip xterm command. We dont have a desktop
+    sed -i 's|tty -s|(exit 0)|g' install.sh && \
+    # Skip Check of Java Version as we have the correct installed and the command will fail
+    sed -i 's|needToInstallJava$|(exit 0)|g' install.sh && \
+    # Skip install of desktop shortcuts - we dont have a desktop
+    sed -i '/for TYPE in UR3 UR5 UR10/,$ d' ./install.sh  && \
+    # Remove commands that are not relevant on docker as we are root user
+    sed -i 's|pkexec ||g' ./install.sh && \
+    sed -i 's|sudo ||g' ./install.sh && \
+    sed -i 's|sudo ||g' ./ursim-certificate-check.sh && \
+    #
+    # Install URSim
+    ./install.sh && \
+    #
+    echo "Installed URSim"
+
+RUN \
+    echo "**** Clean Up ****" && \
+    rm -rf \
+    /tmp/* \
+    /var/lib/apt/lists/* \
+    /var/tmp/*
+
+# Copy ursim run service script
+COPY ursim /etc/services.d/ursim
+COPY safety.conf.UR5 /ursim/.urcontrol/
+# Expose ports
+# Guacamole web browser viewer
+EXPOSE 8080
+# VNC viewer
+EXPOSE 3389
+# Modbus Port
+EXPOSE 502
+# Interface Ports
+EXPOSE 29999
+EXPOSE 30001-30004
+
+# Mount Volumes
+VOLUME /ursim
+
+ENTRYPOINT ["/init"]

.github/dockerursim/build_and_run_docker_ursim.sh

@@ -0,0 +1,17 @@
+#!/bin/bash
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+
+docker build ${DIR} -t mydockerursim
+docker volume create dockerursim
+docker run --name="mydockerursim" -d \
+  -e ROBOT_MODEL=UR5 \
+  -p 8080:8080 \
+  -p 29999:29999 \
+  -p 30001-30004:30001-30004 \
+  -v "${DIR}/.vol":/ursim/programs \
+  -v dockursim:/ursim \
+  --privileged \
+  --cpus=1 \
+  mydockerursim

.github/dockerursim/safety.conf.UR5

@@ -0,0 +1,101 @@
+# Beware: This file is auto-generated from PolyScope.
+# NOTE: The SafetyParameters section is protected by a CRC checksum, please use the supplied tool
+
+## SafetyParameters ##
+[NormalModeSafetyLimits]
+maxTcpSpeed = 1.5
+maxForce = 150.0
+maxElbowSpeed = 1.5
+maxElbowForce = 150.0
+maxStoppingDistance = 0.5
+maxStoppingTime = 0.4
+maxPower = 300.0
+maxMomentum = 25.0
+maxJointSpeed = [3.3415926, 3.3415926, 3.3415926, 3.3415926, 3.3415926, 3.3415926]
+minJointPosition = [6.2308254, 6.2308254, 6.2308254, 6.2308254, 6.2308254, 6.2308254]
+maxJointPosition = [0.05235988, 0.05235988, 0.05235988, 0.05235988, 0.05235988, 0.05235988]
+minJointRevolutions = [-2, -2, -2, -2, -2, -2]
+maxJointRevolutions = [1, 1, 1, 1, 1, 1]
+plane0 = [0.0, 0.0, 0.0, 0.0, 0]
+plane1 = [0.0, 0.0, 0.0, 0.0, 0]
+plane2 = [0.0, 0.0, 0.0, 0.0, 0]
+plane3 = [0.0, 0.0, 0.0, 0.0, 0]
+plane4 = [0.0, 0.0, 0.0, 0.0, 0]
+plane5 = [0.0, 0.0, 0.0, 0.0, 0]
+plane6 = [0.0, 0.0, 0.0, 0.0, 0]
+plane7 = [0.0, 0.0, 0.0, 0.0, 0]
+tcpOrientationVector = [0.0, 0.0, 1.0]
+maximumTcpOrientationDeviation = 6.2831855
+
+[ReducedModeSafetyLimits]
+maxTcpSpeed = 0.75
+maxForce = 120.0
+maxElbowSpeed = 0.75
+maxElbowForce = 120.0
+maxStoppingDistance = 0.3
+maxStoppingTime = 0.3
+maxPower = 200.0
+maxMomentum = 10.0
+maxJointSpeed = [3.3415926, 3.3415926, 3.3415926, 3.3415926, 3.3415926, 3.3415926]
+minJointPosition = [6.2308254, 6.2308254, 6.2308254, 6.2308254, 6.2308254, 6.2308254]
+maxJointPosition = [0.05235988, 0.05235988, 0.05235988, 0.05235988, 0.05235988, 0.05235988]
+minJointRevolutions = [-2, -2, -2, -2, -2, -2]
+maxJointRevolutions = [1, 1, 1, 1, 1, 1]
+plane0 = [0.0, 0.0, 0.0, 0.0, 0]
+plane1 = [0.0, 0.0, 0.0, 0.0, 0]
+plane2 = [0.0, 0.0, 0.0, 0.0, 0]
+plane3 = [0.0, 0.0, 0.0, 0.0, 0]
+plane4 = [0.0, 0.0, 0.0, 0.0, 0]
+plane5 = [0.0, 0.0, 0.0, 0.0, 0]
+plane6 = [0.0, 0.0, 0.0, 0.0, 0]
+plane7 = [0.0, 0.0, 0.0, 0.0, 0]
+tcpOrientationVector = [0.0, 0.0, 1.0]
+maximumTcpOrientationDeviation = 6.2831855
+
+[MiscConfiguration]
+teach_pendant = 1
+euromap67 = 0
+
+[SafetyIOConfiguration]
+emergencyStopInputA = 255
+emergencyStopInputB = 255
+reducedModeInputA = 255
+reducedModeInputB = 255
+safeguardStopResetInputA = 0
+safeguardStopResetInputB = 1
+threePositionEnablingInputA = 255
+threePositionEnablingInputB = 255
+operationalModeInputA = 255
+operationalModeInputB = 255
+systemEmergencyStopOutputA = 255
+systemEmergencyStopOutputB = 255
+robotMovingOutputA = 255
+robotMovingOutputB = 255
+robotNotStoppingOutputA = 255
+robotNotStoppingOutputB = 255
+reducedModeOutputA = 255
+reducedModeOutputB = 255
+notReducedModeOutputA = 255
+notReducedModeOutputB = 255
+
+[ReducedModeTriggerPlanes]
+plane0 = [0.0, 0.0, 0.0, 0.0, 0]
+plane1 = [0.0, 0.0, 0.0, 0.0, 0]
+plane2 = [0.0, 0.0, 0.0, 0.0, 0]
+plane3 = [0.0, 0.0, 0.0, 0.0, 0]
+plane4 = [0.0, 0.0, 0.0, 0.0, 0]
+plane5 = [0.0, 0.0, 0.0, 0.0, 0]
+plane6 = [0.0, 0.0, 0.0, 0.0, 0]
+plane7 = [0.0, 0.0, 0.0, 0.0, 0]
+
+[WorkpieceConfiguration]
+toolSpheres = [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]]
+toolDirectionInclination = 0.0
+toolDirectionAzimuth = 0.0
+
+
+## SafetyParameters ##
+[Checksum]
+safetyParameters = 3478627865
+majorVersion = 5
+minorVersion = 0

.github/dockerursim/ursim/run

@@ -0,0 +1,16 @@
+#!/bin/execlineb -P
+
+s6-envdir -fn -- /var/run/s6/container_environment
+importas -i ROBOT_MODEL ROBOT_MODEL
+
+# Redirect stderr to stdout.
+fdmove -c 2 1
+
+# Wait until openbox is running
+if { s6-svwait -t 10000 -U /var/run/s6/services/openbox/ }
+
+# Set env
+s6-env DISPLAY=:1
+
+# Execute URSim
+/ursim/start-ursim.sh ${ROBOT_MODEL}

.github/workflows/ci.yml

@@ -21,12 +21,21 @@ jobs:
           - ROS_DISTRO: kinetic
             UPSTREAM_WORKSPACE: .ci.rosinstall
             ROS_REPO: main
+            DOCKER_RUN_OPTS: --network bridge
+            BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 172.17.0.2'
+            IMMEDIATE_TEST_OUTPUT: true
           - ROS_DISTRO: melodic
             UPSTREAM_WORKSPACE: .ci.rosinstall
             ROS_REPO: main
+            DOCKER_RUN_OPTS: --network bridge
+            BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 172.17.0.2'
+            IMMEDIATE_TEST_OUTPUT: true
 
     steps:
       - uses: actions/checkout@v1
+      - name: start ursim
+        run: |
+          .github/dockerursim/build_and_run_docker_ursim.sh
       - uses: 'ros-industrial/industrial_ci@master'
         env: ${{matrix.env}}
 

ur_robot_driver/CMakeLists.txt

@@ -141,6 +141,18 @@ add_executable(robot_state_helper
 target_link_libraries(robot_state_helper ${catkin_LIBRARIES} ur_robot_driver)
 add_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+
+  add_rostest(test/driver.test)
+
+  catkin_add_gtest(test_rtde_client
+    test/test_rtde_client.cpp
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
+  target_link_libraries(test_rtde_client ${catkin_LIBRARIES} ur_robot_driver)
+endif()
+
+
 install(TARGETS ur_robot_driver ur_robot_driver_plugin ur_robot_driver_node robot_state_helper dashboard_client
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -150,7 +162,7 @@ install(TARGETS ur_robot_driver ur_robot_driver_plugin ur_robot_driver_node robo
 install(PROGRAMS scripts/tool_communication
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-install(DIRECTORY config launch
+install(DIRECTORY config launch resources
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 

ur_robot_driver/include/ur_robot_driver/rtde/rtde_writer.h

@@ -55,7 +55,16 @@ class RTDEWriter
    * \param recipe The recipe to use for communication
    */
   RTDEWriter(comm::URStream<RTDEPackage>* stream, const std::vector<std::string>& recipe);
-  ~RTDEWriter() = default;
+
+  ~RTDEWriter()
+  {
+    running_ = false;
+    std::this_thread::sleep_for(std::chrono::seconds(5));
+    if (writer_thread_.joinable())
+    {
+      writer_thread_.join();
+    }
+  }
   /*!
    * \brief Starts the writer thread, which periodically clears the queue to write packages to the
    * robot.
@@ -120,6 +129,7 @@ class RTDEWriter
   uint8_t recipe_id_;
   moodycamel::BlockingReaderWriterQueue<std::unique_ptr<DataPackage>> queue_;
   std::thread writer_thread_;
+  bool running_;
 };
 
 }  // namespace rtde_interface

ur_robot_driver/package.xml

@@ -51,6 +51,8 @@
   <exec_depend>ur_e_description</exec_depend>
   <exec_depend>velocity_controllers</exec_depend>
 
+  <test_depend>rostest</test_depend>
+
   <export>
     <hardware_interface plugin="${prefix}/hardware_interface_plugin.xml" />
     <rosdoc config="doc/rosdoc.yaml" />

ur_robot_driver/src/rtde/rtde_client.cpp

@@ -323,6 +323,13 @@ std::vector<std::string> RTDEClient::readRecipe(const std::string& recipe_file)
 {
   std::vector<std::string> recipe;
   std::ifstream file(recipe_file);
+  if (file.fail())
+  {
+    std::stringstream msg;
+    msg << "Opening file '" << recipe_file << "' failed with error: " << strerror(errno);
+    LOG_ERROR("%s", msg.str().c_str());
+    throw UrException(msg.str());
+  }
   std::string line;
   while (std::getline(file, line))
   {