diff --git a/.github/dockerursim/.vol/default.installation b/.github/dockerursim/.vol/default.installation new file mode 100644 index 000000000..0f85fab88 Binary files /dev/null and b/.github/dockerursim/.vol/default.installation differ diff --git a/.github/dockerursim/.vol/default.variables b/.github/dockerursim/.vol/default.variables new file mode 100644 index 000000000..86a46f302 --- /dev/null +++ b/.github/dockerursim/.vol/default.variables @@ -0,0 +1,2 @@ +# +#Thu May 28 16:20:13 BST 2020 diff --git a/.github/dockerursim/.vol/programs.UR5 b/.github/dockerursim/.vol/programs.UR5 new file mode 100755 index 000000000..ebd12373d --- /dev/null +++ b/.github/dockerursim/.vol/programs.UR5 @@ -0,0 +1 @@ +/ursim/programs.UR5 \ No newline at end of file diff --git a/.github/dockerursim/Dockerfile b/.github/dockerursim/Dockerfile new file mode 100644 index 000000000..9d4b2a54c --- /dev/null +++ b/.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"] diff --git a/.github/dockerursim/build_and_run_docker_ursim.sh b/.github/dockerursim/build_and_run_docker_ursim.sh new file mode 100755 index 000000000..981138736 --- /dev/null +++ b/.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 diff --git a/.github/dockerursim/safety.conf.UR5 b/.github/dockerursim/safety.conf.UR5 new file mode 100644 index 000000000..908b382f4 --- /dev/null +++ b/.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 diff --git a/.github/dockerursim/ursim/run b/.github/dockerursim/ursim/run new file mode 100644 index 000000000..4021c48fc --- /dev/null +++ b/.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} \ No newline at end of file diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 16d86e9b7..66fe2a735 100644 --- a/.github/workflows/ci.yml +++ b/.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}} diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index eb89d7cf5..48f69295f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/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}) diff --git a/ur_robot_driver/include/ur_robot_driver/rtde/rtde_writer.h b/ur_robot_driver/include/ur_robot_driver/rtde/rtde_writer.h index 6fc186ba9..93881c61f 100644 --- a/ur_robot_driver/include/ur_robot_driver/rtde/rtde_writer.h +++ b/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* stream, const std::vector& 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> queue_; std::thread writer_thread_; + bool running_; }; } // namespace rtde_interface diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 78f19c4de..431b72c71 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -51,6 +51,8 @@ ur_e_description velocity_controllers + rostest + diff --git a/ur_robot_driver/src/rtde/rtde_client.cpp b/ur_robot_driver/src/rtde/rtde_client.cpp index 60a7a52e9..decb22b99 100644 --- a/ur_robot_driver/src/rtde/rtde_client.cpp +++ b/ur_robot_driver/src/rtde/rtde_client.cpp @@ -323,6 +323,13 @@ std::vector RTDEClient::readRecipe(const std::string& recipe_file) { std::vector 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)) { diff --git a/ur_robot_driver/src/rtde/rtde_writer.cpp b/ur_robot_driver/src/rtde/rtde_writer.cpp index e4f15e5c5..0e8f33f69 100644 --- a/ur_robot_driver/src/rtde/rtde_writer.cpp +++ b/ur_robot_driver/src/rtde/rtde_writer.cpp @@ -32,13 +32,14 @@ namespace ur_driver namespace rtde_interface { RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector& recipe) - : stream_(stream), recipe_(recipe), queue_{ 32 } + : stream_(stream), recipe_(recipe), queue_{ 32 }, running_(false) { } void RTDEWriter::init(uint8_t recipe_id) { recipe_id_ = recipe_id; + running_ = true; writer_thread_ = std::thread(&RTDEWriter::run, this); } @@ -48,13 +49,16 @@ void RTDEWriter::run() size_t size; size_t written; std::unique_ptr package; - while (true) + while (running_) { - queue_.waitDequeue(package); - package->setRecipeID(recipe_id_); - size = package->serializePackage(buffer); - stream_->write(buffer, size, written); + if (queue_.waitDequeTimed(package, 1000000)) + { + package->setRecipeID(recipe_id_); + size = package->serializePackage(buffer); + stream_->write(buffer, size, written); + } } + LOG_DEBUG("Write thread ended."); } bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) diff --git a/ur_robot_driver/test/driver.test b/ur_robot_driver/test/driver.test new file mode 100644 index 000000000..2e9096abb --- /dev/null +++ b/ur_robot_driver/test/driver.test @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + diff --git a/ur_robot_driver/test/io_test.py b/ur_robot_driver/test/io_test.py new file mode 100755 index 000000000..4c705374b --- /dev/null +++ b/ur_robot_driver/test/io_test.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python +import sys +import time +import unittest + + +import rospy +import rostopic + + +PKG = 'ur_robot_driver' +NAME = 'io_test' + + +from ur_msgs.srv import SetIO, SetIORequest +from ur_msgs.msg import IOStates + + +class IOTest(unittest.TestCase): + def __init__(self, *args): + super(IOTest, self).__init__(*args) + rospy.init_node('io_test') + + timeout = 10 + + self.service_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO) + try: + self.service_client.wait_for_service(timeout) + except rospy.exceptions.ROSException as err: + self.fail( + "Could not reach SetIO service. Make sure that the driver is actually running." + " Msg: {}".format(err)) + + def test_set_io(self): + """Test to set an IO and check whether it has been set.""" + + maximum_messages = 5 + pin = 0 + self.assertEqual(maximum_messages, 5) + + self.service_client(1, pin, 0) + messages = 0 + pin_state = True + + while(pin_state): + if messages >= maximum_messages: + self.fail("Could not read desired state after {} messages.".format(maximum_messages)) + io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates) + pin_state = io_state.digital_out_states[pin].state + messages += 1 + self.assertEqual(pin_state, 0) + + self.service_client(SetIORequest.FUN_SET_DIGITAL_OUT, pin, 1) + messages = 0 + pin_state = False + + while(not pin_state): + if messages >= maximum_messages: + self.fail("Could not read desired state after {} messages.".format(maximum_messages)) + io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates) + pin_state = io_state.digital_out_states[pin].state + messages += 1 + self.assertEqual(pin_state, 1) + + +if __name__ == '__main__': + import rostest + rostest.run(PKG, NAME, IOTest, sys.argv) diff --git a/ur_robot_driver/test/resources/empty.txt b/ur_robot_driver/test/resources/empty.txt new file mode 100644 index 000000000..8d1c8b69c --- /dev/null +++ b/ur_robot_driver/test/resources/empty.txt @@ -0,0 +1 @@ + diff --git a/ur_robot_driver/test/resources/rtde_input_recipe.txt b/ur_robot_driver/test/resources/rtde_input_recipe.txt new file mode 100644 index 000000000..c00dc059d --- /dev/null +++ b/ur_robot_driver/test/resources/rtde_input_recipe.txt @@ -0,0 +1,12 @@ +speed_slider_mask +speed_slider_fraction +standard_digital_output_mask +standard_digital_output +configurable_digital_output_mask +configurable_digital_output +tool_digital_output_mask +tool_digital_output +standard_analog_output_mask +standard_analog_output_type +standard_analog_output_0 +standard_analog_output_1 diff --git a/ur_robot_driver/test/resources/rtde_output_recipe.txt b/ur_robot_driver/test/resources/rtde_output_recipe.txt new file mode 100644 index 000000000..8d10be849 --- /dev/null +++ b/ur_robot_driver/test/resources/rtde_output_recipe.txt @@ -0,0 +1,27 @@ +timestamp +actual_q +actual_qd +speed_scaling +target_speed_fraction +runtime_state +actual_TCP_force +actual_TCP_pose +actual_digital_input_bits +actual_digital_output_bits +standard_analog_input0 +standard_analog_input1 +standard_analog_output0 +standard_analog_output1 +analog_io_types +tool_mode +tool_analog_input_types +tool_analog_input0 +tool_analog_input1 +tool_output_voltage +tool_output_current +tool_temperature +robot_mode +safety_mode +robot_status_bits +safety_status_bits +actual_current diff --git a/ur_robot_driver/test/switch_on_test.py b/ur_robot_driver/test/switch_on_test.py new file mode 100755 index 000000000..07b07b300 --- /dev/null +++ b/ur_robot_driver/test/switch_on_test.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python +import sys +import time +import unittest + + +import rospy +import rostopic + + +PKG = 'ur_robot_driver' +NAME = 'switch_on_test' + +import actionlib +from ur_dashboard_msgs.msg import SetModeAction, SetModeGoal, RobotMode + + +class IOTest(unittest.TestCase): + def __init__(self, *args): + super(IOTest, self).__init__(*args) + + rospy.init_node('switch_on_robot') + self.client = actionlib.SimpleActionClient( + '/ur_hardware_interface/set_mode', SetModeAction) + timeout = rospy.Duration(30) + try: + self.client.wait_for_server(timeout) + except rospy.exceptions.ROSException as err: + self.fail( + "Could not reach set_mode action. Make sure that the driver is actually running." + " Msg: {}".format(err)) + + def test_switch_on(self): + """Test to set an IO and check whether it has been set.""" + + goal = SetModeGoal() + goal.target_robot_mode = RobotMode.RUNNING + goal.play_program = False # we use headless mode during tests + + self.client.send_goal(goal) + self.client.wait_for_result() + + self.assertTrue(self.client.get_result().success) + + +if __name__ == '__main__': + import rostest + rostest.run(PKG, NAME, IOTest, sys.argv) diff --git a/ur_robot_driver/test/test_rtde_client.cpp b/ur_robot_driver/test/test_rtde_client.cpp new file mode 100644 index 000000000..686e5a072 --- /dev/null +++ b/ur_robot_driver/test/test_rtde_client.cpp @@ -0,0 +1,76 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner mauch@fzi.de + * \date 2020-07-09 + * + */ +//---------------------------------------------------------------------- + +#include + +#include + +using namespace ur_driver; + +const std::string ROBOT_IP = "172.17.0.2"; + +TEST(UrRobotDriver, rtde_handshake) +{ + comm::INotifier notifier; + std::string output_recipe = "resources/rtde_output_recipe.txt"; + std::string input_recipe = "resources/rtde_input_recipe.txt"; + rtde_interface::RTDEClient client(ROBOT_IP, notifier, output_recipe, input_recipe); + + EXPECT_TRUE(client.init()); +} + +TEST(UrRobotDriver, rtde_handshake_wrong_ip) +{ + comm::INotifier notifier; + std::string output_recipe = "resources/rtde_output_recipe.txt"; + std::string input_recipe = "resources/rtde_input_recipe.txt"; + rtde_interface::RTDEClient client("1.2.3.4", notifier, output_recipe, input_recipe); + + EXPECT_THROW(client.init(), UrException); +} + +TEST(UrRobotDriver, rtde_handshake_illegal_ip) +{ + comm::INotifier notifier; + std::string output_recipe = "resources/rtde_output_recipe.txt"; + std::string input_recipe = "resources/rtde_input_recipe.txt"; + rtde_interface::RTDEClient client("abcd", notifier, output_recipe, input_recipe); + + EXPECT_THROW(client.init(), UrException); +} + +TEST(UrRobotDriver, no_recipe) +{ + comm::INotifier notifier; + std::string output_recipe = ""; + std::string input_recipe = ""; + EXPECT_THROW(rtde_interface::RTDEClient client(ROBOT_IP, notifier, output_recipe, input_recipe), UrException); +} + +TEST(UrRobotDriver, empty_recipe) +{ + comm::INotifier notifier; + std::string output_recipe = "resources/empty.txt"; + std::string input_recipe = "resources/empty.txt"; + rtde_interface::RTDEClient client(ROBOT_IP, notifier, output_recipe, input_recipe); + + EXPECT_THROW(client.init(), UrException); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/ur_robot_driver/test/trajectory_test.py b/ur_robot_driver/test/trajectory_test.py new file mode 100755 index 000000000..346b8db19 --- /dev/null +++ b/ur_robot_driver/test/trajectory_test.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python +import sys +import time +import unittest + +import rospy +import actionlib +from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal, FollowJointTrajectoryResult +from ur_dashboard_msgs.msg import SetModeAction, SetModeGoal, RobotMode +from std_srvs.srv import Trigger, TriggerRequest +from trajectory_msgs.msg import JointTrajectoryPoint + +PKG = 'ur_robot_driver' +NAME = 'trajectory_test' + + +class TrajectoryTest(unittest.TestCase): + def __init__(self, *args): + super(TrajectoryTest, self).__init__(*args) + rospy.init_node('trajectory_testing_client') + self.client = actionlib.SimpleActionClient( + 'follow_joint_trajectory', FollowJointTrajectoryAction) + timeout = rospy.Duration(30) + try: + self.client.wait_for_server(timeout) + except rospy.exceptions.ROSException as err: + self.fail( + "Could not reach controller action. Make sure that the driver is actually running." + " Msg: {}".format(err)) + + self.init_robot() + + def init_robot(self): + """Make sure the robot is booted and ready to receive commands""" + mode_client = actionlib.SimpleActionClient( + '/ur_hardware_interface/set_mode', SetModeAction) + timeout = rospy.Duration(30) + try: + mode_client.wait_for_server(timeout) + except rospy.exceptions.ROSException as err: + self.fail( + "Could not reach set_mode action. Make sure that the driver is actually running." + " Msg: {}".format(err)) + goal = SetModeGoal() + goal.target_robot_mode = RobotMode.RUNNING + goal.play_program = False # we use headless mode during tests + + mode_client.send_goal(goal) + mode_client.wait_for_result() + + self.assertTrue(mode_client.get_result().success) + + send_program_srv = rospy.ServiceProxy("/ur_hardware_interface/resend_robot_program", Trigger) + send_program_srv.call() + rospy.sleep(5) + + def test_trajectory(self): + """Test robot movement""" + goal = FollowJointTrajectoryGoal() + + goal.trajectory.joint_names = ["elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] + position_list = [[0.0 for i in range(6)]] + position_list.append([-0.5 for i in range(6)]) + position_list.append([-1.0 for i in range(6)]) + duration_list = [6.0, 9.0, 12.0] + + for i, position in enumerate(position_list): + point = JointTrajectoryPoint() + point.positions = position + point.time_from_start = rospy.Duration(duration_list[i]) + goal.trajectory.points.append(point) + + rospy.loginfo("Sending simple goal") + + self.client.send_goal(goal) + self.client.wait_for_result() + + self.assertEqual(self.client.get_result().error_code, + FollowJointTrajectoryResult.SUCCESSFUL) + rospy.loginfo("Received result SUCCESSFUL") + + def test_illegal_trajectory(self): + """Test trajectory server. This is more of a validation test that the testing suite does the + right thing.""" + goal = FollowJointTrajectoryGoal() + + goal.trajectory.joint_names = ["elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] + position_list = [[0.0 for i in range(6)]] + position_list.append([-0.5 for i in range(6)]) + # Create illegal goal by making the second point come earlier than the first + duration_list = [6.0, 3.0] + + for i, position in enumerate(position_list): + point = JointTrajectoryPoint() + point.positions = position + point.time_from_start = rospy.Duration(duration_list[i]) + goal.trajectory.points.append(point) + + rospy.loginfo("Sending illegal goal") + self.client.send_goal(goal) + self.client.wait_for_result() + + # As timings are illegal, we expect the result to be INVALID_GOAL + self.assertEqual(self.client.get_result().error_code, + FollowJointTrajectoryResult.INVALID_GOAL) + rospy.loginfo("Received result INVALID_GOAL") + + def test_scaled_trajectory(self): + """Test robot movement""" + goal = FollowJointTrajectoryGoal() + + goal.trajectory.joint_names = ["elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] + position_list = [[0.0 for i in range(6)]] + position_list.append([-1.0 for i in range(6)]) + duration_list = [6.0, 6.5] + + for i, position in enumerate(position_list): + point = JointTrajectoryPoint() + point.positions = position + point.time_from_start = rospy.Duration(duration_list[i]) + goal.trajectory.points.append(point) + + rospy.loginfo("Sending scaled goal without time restrictions") + self.client.send_goal(goal) + self.client.wait_for_result() + + self.assertEqual(self.client.get_result().error_code, + FollowJointTrajectoryResult.SUCCESSFUL) + rospy.loginfo("Received result SUCCESSFUL") + + # Now do the same again, but with a goal time constraint + rospy.loginfo("Sending scaled goal with time restrictions") + goal.goal_time_tolerance = rospy.Duration(0.01) + self.client.send_goal(goal) + self.client.wait_for_result() + + self.assertEqual(self.client.get_result().error_code, + FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED) + rospy.loginfo("Received result GOAL_TOLERANCE_VIOLATED") + + +if __name__ == '__main__': + import rostest + rostest.run(PKG, NAME, TrajectoryTest, sys.argv)