Added watchdog configuration for the reverse socket (#178)

This enables the possibility to configure the read timeout for the
reverse socket running in the external control script

Separated control modes into realtime and non realtime to have different
possible configurations for the watchdog

Added test to verify the changes

Updated test and examples to coincide with the new changes

Author: urmahp

CMakeLists.txt

@@ -43,6 +43,7 @@ add_library(urcl SHARED
     src/ur/calibration_checker.cpp
     src/ur/dashboard_client.cpp
     src/ur/tool_communication.cpp
+    src/ur/robot_receive_timeout.cpp
     src/ur/version_information.cpp
     src/rtde/rtde_writer.cpp
     src/default_log_handler.cpp

examples/full_driver.cpp

@@ -110,8 +110,6 @@ int main(int argc, char* argv[])
   const bool HEADLESS = true;
   g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
                                  std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
-  g_my_driver->setKeepaliveCount(5);  // This is for example purposes only. This will make the example running more
-                                      // reliable on non-realtime systems. Do not use this in productive applications.
 
   // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
   // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
@@ -153,7 +151,10 @@ int main(int argc, char* argv[])
         increment = 0.02;
       }
       g_joint_positions[5] += increment;
-      bool ret = g_my_driver->writeJointCommand(g_joint_positions, comm::ControlMode::MODE_SERVOJ);
+      // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more
+      // reliable on non-realtime systems. Use with caution in productive applications.
+      bool ret = g_my_driver->writeJointCommand(g_joint_positions, comm::ControlMode::MODE_SERVOJ,
+                                                RobotReceiveTimeout::millisec(100));
       if (!ret)
       {
         URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
@@ -166,6 +167,7 @@ int main(int argc, char* argv[])
       URCL_LOG_WARN("Could not get fresh data package from robot");
     }
   }
+  g_my_driver->stopControl();
   URCL_LOG_INFO("Movement done");
   return 0;
 }

examples/spline_example.cpp

@@ -177,8 +177,6 @@ int main(int argc, char* argv[])
   const bool HEADLESS = true;
   g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
                                  std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
-  g_my_driver->setKeepaliveCount(5);  // This is for example purposes only. This will make the example running more
-                                      // reliable on non-realtime systems. Do not use this in productive applications.
 
   g_my_driver->registerTrajectoryDoneCallback(&handleTrajectoryState);
 
@@ -211,8 +209,7 @@ int main(int argc, char* argv[])
                                             4.00000000e+00 };
 
   bool ret = false;
-  URCL_LOG_INFO("Switch to Forward mode");
-  ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+  ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP);
   if (!ret)
   {
     std::stringstream lastq;
@@ -232,11 +229,11 @@ int main(int argc, char* argv[])
     p_i[joint_to_control] = s_pos[i];
     p.push_back(p_i);
 
-    vector6d_t v_i;
+    vector6d_t v_i = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
     v_i[joint_to_control] = s_vel[i];
     v.push_back(v_i);
 
-    vector6d_t a_i;
+    vector6d_t a_i = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
     a_i[joint_to_control] = s_acc[i];
     a.push_back(a_i);
 
@@ -259,7 +256,7 @@ int main(int argc, char* argv[])
         std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
         throw std::runtime_error(error_msg);
       }
-      bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+      ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP);
 
       if (!ret)
       {
@@ -276,7 +273,6 @@ int main(int argc, char* argv[])
 
   // QUINTIC
   SendTrajectory(p, v, a, time, true);
-  ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
 
   g_trajectory_running = true;
   while (g_trajectory_running)
@@ -291,7 +287,7 @@ int main(int argc, char* argv[])
         std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
         throw std::runtime_error(error_msg);
       }
-      bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+      ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP);
 
       if (!ret)
       {
@@ -306,7 +302,7 @@ int main(int argc, char* argv[])
 
   URCL_LOG_INFO("QUINTIC Movement done");
 
-  ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+  ret = g_my_driver->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP);
   if (!ret)
   {
     std::stringstream lastq;
@@ -315,5 +311,6 @@ int main(int argc, char* argv[])
                    lastq.str().c_str());
     return 1;
   }
+  g_my_driver->stopControl();
   return 0;
 }

examples/tool_contact_example.cpp

@@ -127,8 +127,6 @@ int main(int argc, char* argv[])
   const bool HEADLESS = true;
   g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
                                  std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
-  g_my_driver->setKeepaliveCount(5);  // This is for example purposes only. This will make the example running more
-                                      // reliable on non-realtime systems. Do not use this in productive applications.
 
   g_my_driver->registerToolContactResultCallback(&handleToolContactResult);
 
@@ -155,7 +153,10 @@ int main(int argc, char* argv[])
     std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
     if (data_pkg)
     {
-      bool ret = g_my_driver->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL);
+      // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more
+      // reliable on non-realtime systems. Use with caution in productive applications.
+      bool ret =
+          g_my_driver->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL, RobotReceiveTimeout::millisec(100));
       if (!ret)
       {
         URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");

include/ur_client_library/comm/control_mode.h

@@ -29,6 +29,9 @@
 #ifndef UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED
 #define UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED
 
+#include <algorithm>
+#include <vector>
+
 namespace urcl
 {
 namespace comm
@@ -48,8 +51,59 @@ enum class ControlMode : int32_t
   MODE_POSE = 5,            ///< Set when cartesian pose control is active.
   MODE_FREEDRIVE = 6,       ///< Set when freedrive mode is active.
   MODE_TOOL_IN_CONTACT =
-      7  ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact()
+      7,  ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact()
+  END     ///< This is not an actual control mode, but used internally to get the number of control modes
 };
+
+/*!
+ * \brief Class used to separate the control modes into realtime and non realtime.
+ */
+class ControlModeTypes
+{
+public:
+  // Control modes that require realtime communication
+  static const inline std::vector<ControlMode> REALTIME_CONTROL_MODES = {
+    ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE
+  };
+
+  // Control modes that doesn't require realtime communication
+  static const inline std::vector<ControlMode> NON_REALTIME_CONTROL_MODES = { ControlMode::MODE_IDLE,
+                                                                              ControlMode::MODE_FORWARD,
+                                                                              ControlMode::MODE_FREEDRIVE };
+
+  // Control modes which doesn't move the robot, meaning they are neither realtime or non realtime
+  static const inline std::vector<ControlMode> STATIONARY_CONTROL_MODES = { ControlMode::MODE_STOPPED,
+                                                                            ControlMode::MODE_UNINITIALIZED,
+                                                                            ControlMode::MODE_TOOL_IN_CONTACT };
+
+  /*!
+   * \brief Check if the control mode is realtime
+   *
+   * \param control_mode Current control mode
+   *
+   * \returns true if the control mode is realtime, false otherwise
+   */
+  static bool is_control_mode_realtime(ControlMode control_mode)
+  {
+    return (std::find(ControlModeTypes::REALTIME_CONTROL_MODES.begin(), ControlModeTypes::REALTIME_CONTROL_MODES.end(),
+                      control_mode) != ControlModeTypes::REALTIME_CONTROL_MODES.end());
+  }
+
+  /*!
+   * \brief Check if the control mode is non realtime
+   *
+   * \param control_mode Current control mode
+   *
+   * \returns true if the control mode is non realtime, false otherwise
+   */
+  static bool is_control_mode_non_realtime(ControlMode control_mode)
+  {
+    return (std::find(ControlModeTypes::NON_REALTIME_CONTROL_MODES.begin(),
+                      ControlModeTypes::NON_REALTIME_CONTROL_MODES.end(),
+                      control_mode) != ControlModeTypes::NON_REALTIME_CONTROL_MODES.end());
+  }
+};
+
 }  // namespace comm
 }  // namespace urcl
 

include/ur_client_library/control/reverse_interface.h

@@ -33,6 +33,7 @@
 #include "ur_client_library/comm/control_mode.h"
 #include "ur_client_library/types.h"
 #include "ur_client_library/log.h"
+#include "ur_client_library/ur/robot_receive_timeout.h"
 #include <cstring>
 #include <endian.h>
 #include <condition_variable>
@@ -76,8 +77,10 @@ class ReverseInterface
    *
    * \param port Port the Server is started on
    * \param handle_program_state Function handle to a callback on program state changes.
+   * \param step_time The robots step time
    */
-  ReverseInterface(uint32_t port, std::function<void(bool)> handle_program_state);
+  ReverseInterface(uint32_t port, std::function<void(bool)> handle_program_state,
+                   std::chrono::milliseconds step_time = std::chrono::milliseconds(8));
 
   /*!
    * \brief Disconnects possible clients so the reverse interface object can be safely destroyed.
@@ -90,39 +93,53 @@ class ReverseInterface
    * \param positions A vector of joint targets for the robot
    * \param control_mode Control mode assigned to this command. See documentation of comm::ControlMode
    * for details on possible values.
+   * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external
+   * control script on the robot. Use with caution when dealing with realtime commands as the robot
+   * expects to get a new control signal each control cycle. Note the timeout cannot be higher than 1 second for
+   * realtime commands.
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  virtual bool write(const vector6d_t* positions, const comm::ControlMode control_mode = comm::ControlMode::MODE_IDLE);
+  virtual bool write(const vector6d_t* positions, const comm::ControlMode control_mode = comm::ControlMode::MODE_IDLE,
+                     const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(20));
 
   /*!
    * \brief Writes needed information to the robot to be read by the URScript program.
    *
    * \param trajectory_action 1 if a trajectory is to be started, -1 if it should be stopped
    * \param point_number The number of points of the trajectory to be executed
+   * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external
+   * control script on the robot. If you want to make the read function blocking then use RobotReceiveTimeout::off()
+   * function to create the RobotReceiveTimeout object
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0);
+  bool
+  writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0,
+                                const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200));
 
   /*!
    * \brief Writes needed information to the robot to be read by the URScript program.
    *
    * \param freedrive_action 1 if freedrive mode is to be started, -1 if it should be stopped and 0 to keep it running
+   * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external
+   * control script on the robot. If you want to make the read function blocking then use RobotReceiveTimeout::off()
+   * function to create the RobotReceiveTimeout object
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  bool writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action);
+  bool
+  writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action,
+                               const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200));
 
   /*!
    * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot.
    *
    * \param count Number of allowed timeout reads on the robot.
    */
-  virtual void setKeepaliveCount(const uint32_t& count)
-  {
-    keepalive_count_ = count;
-  }
+  [[deprecated("Set keepaliveCount is deprecated, instead use the robot receive timeout directly in the write "
+               "commands.")]] virtual void
+  setKeepaliveCount(const uint32_t count);
 
 protected:
   virtual void connectionCallback(const int filedescriptor);
@@ -145,7 +162,10 @@ class ReverseInterface
   static const int MAX_MESSAGE_LENGTH = 8;
 
   std::function<void(bool)> handle_program_state_;
+  std::chrono::milliseconds step_time_;
+
   uint32_t keepalive_count_;
+  bool keep_alive_count_modified_deprecated_;
 };
 
 }  // namespace control

include/ur_client_library/ur/robot_receive_timeout.h

@@ -0,0 +1,96 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2023 Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#pragma once
+
+#include "ur_client_library/comm/control_mode.h"
+#include <chrono>
+
+namespace urcl
+{
+/*!
+ * \brief RobotReceiveTimeout class containing a timeout configuration
+ *
+ * This robot receive timeout is used to configure the read timeout for the reverse socket running in the external
+ * control script. The read timeout is the number of milliseconds until the read action times out. A timeout of 0 or
+ * negative number indicates that the function should not return until a read is completed, this will make the read
+ * function on the robot blocking. This can be set using the function off().
+ *
+ */
+class RobotReceiveTimeout
+{
+public:
+  static constexpr std::chrono::milliseconds MAX_RT_RECEIVE_TIMEOUT_MS = std::chrono::milliseconds(200);
+  RobotReceiveTimeout() = delete;
+  ~RobotReceiveTimeout() = default;
+
+  /*!
+   * \brief Create a RobotReceiveTimeout object with a specific timeout given in milliseconds
+   *
+   * \param milliseconds robot receive timeout
+   *
+   * \returns RobotReceiveTimeout object
+   */
+  static RobotReceiveTimeout millisec(const unsigned int milliseconds = 20);
+
+  /*!
+   * \brief Create a RobotReceiveTimeout object with a specific timeout given in seconds
+   *
+   * \param seconds robot receive timeout
+   *
+   * \returns RobotReceiveTimeout object
+   */
+  static RobotReceiveTimeout sec(const float seconds = 0.02);
+
+  /*!
+   * \brief Creates a RobotReceiveTimeout object with no timeout, this will make the read function on the robot blocking
+   *
+   * \returns RobotReceiveTimeout object
+   */
+  static RobotReceiveTimeout off();
+
+  /*!
+   * \brief Helper function to verify that the robot receive timeout is configured appropriately given the current
+   * control mode
+   *
+   * \param control_mode current control mode
+   * \param step_time The robots step time
+   *
+   * \returns receive timeout in milliseconds
+   */
+  int verifyRobotReceiveTimeout(const comm::ControlMode control_mode, const std::chrono::milliseconds step_time) const;
+
+  std::chrono::milliseconds timeout_;
+
+private:
+  RobotReceiveTimeout(std::chrono::milliseconds timeout);
+};
+
+}  // namespace urcl