Moved verifyWatchdogTimeout to watchdog class

Moved timeout check to the reverse interface

Changed timeout from float to std::chrono::milliseconds

examples/full_driver.cpp

@@ -153,7 +153,8 @@ int main(int argc, char* argv[])
       g_joint_positions[5] += increment;
       // Setting the Watchdog 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, Watchdog::sec(0.1));
+      bool ret = g_my_driver->writeJointCommand(g_joint_positions, comm::ControlMode::MODE_SERVOJ,
+                                                Watchdog::millisec(std::chrono::milliseconds(100)));
       if (!ret)
       {
         URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");

examples/tool_contact_example.cpp

@@ -155,7 +155,8 @@ int main(int argc, char* argv[])
     {
       // Setting the Watchdog 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, Watchdog::sec(0.1));
+      bool ret = g_my_driver->writeJointCommand(tcp_speed, comm::ControlMode::MODE_SPEEDL,
+                                                Watchdog::millisec(std::chrono::milliseconds(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

@@ -30,6 +30,7 @@
 #define UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED
 
 #include <algorithm>
+#include <vector>
 
 namespace urcl
 {
@@ -60,12 +61,14 @@ class ControlModeTypes
 {
 public:
   // Control modes that require realtime communication
-  static constexpr ControlMode REALTIME_CONTROL_MODES[] = { ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ,
-                                                            ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE };
+  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 constexpr ControlMode NON_REALTIME_CONTROL_MODES[] = { ControlMode::MODE_IDLE, ControlMode::MODE_FORWARD,
-                                                                ControlMode::MODE_FREEDRIVE };
+  static const inline std::vector<ControlMode> NON_REALTIME_CONTROL_MODES = { ControlMode::MODE_IDLE,
+                                                                              ControlMode::MODE_FORWARD,
+                                                                              ControlMode::MODE_FREEDRIVE };
 
   /*!
    * \brief Check if the control mode is realtime
@@ -76,10 +79,8 @@ class ControlModeTypes
    */
   static bool is_control_mode_realtime(ControlMode control_mode)
   {
-    int size = sizeof(ControlModeTypes::REALTIME_CONTROL_MODES) / sizeof(*ControlModeTypes::REALTIME_CONTROL_MODES);
-
-    return (std::find(ControlModeTypes::REALTIME_CONTROL_MODES, ControlModeTypes::REALTIME_CONTROL_MODES + size,
-                      control_mode) != ControlModeTypes::REALTIME_CONTROL_MODES + size);
+    return (std::find(ControlModeTypes::REALTIME_CONTROL_MODES.begin(), ControlModeTypes::REALTIME_CONTROL_MODES.end(),
+                      control_mode) != ControlModeTypes::REALTIME_CONTROL_MODES.end());
   }
 
   /*!
@@ -91,10 +92,9 @@ class ControlModeTypes
    */
   static bool is_control_mode_non_realtime(ControlMode control_mode)
   {
-    int size =
-        sizeof(ControlModeTypes::NON_REALTIME_CONTROL_MODES) / sizeof(*ControlModeTypes::NON_REALTIME_CONTROL_MODES);
-    return (std::find(ControlModeTypes::NON_REALTIME_CONTROL_MODES, ControlModeTypes::NON_REALTIME_CONTROL_MODES + size,
-                      control_mode) != ControlModeTypes::NON_REALTIME_CONTROL_MODES + size);
+    return (std::find(ControlModeTypes::NON_REALTIME_CONTROL_MODES.begin(),
+                      ControlModeTypes::NON_REALTIME_CONTROL_MODES.end(),
+                      control_mode) != ControlModeTypes::NON_REALTIME_CONTROL_MODES.end());
   }
 };
 

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/watchdog.h"
 #include <cstring>
 #include <endian.h>
 #include <condition_variable>
@@ -77,7 +78,8 @@ class ReverseInterface
    * \param port Port the Server is started on
    * \param handle_program_state Function handle to a callback on program state changes.
    */
-  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,40 +92,42 @@ 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 read_timeout Read timeout for the reverse socket on the robot. It is The number of seconds until the read
-   * action times out (float). A timeout of 0 or negative number indicates that the function should not return until a
-   * read is completed
+   * \param watchdog 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,
-                     const float& read_timeout = 0.02);
+                     const Watchdog& watchdog = Watchdog::millisec(std::chrono::milliseconds(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 read_timeout Read timeout for the reverse socket on the robot. It is The number of seconds until the read
-   * action times out (float). A timeout of 0 or negative number indicates that the function should not return until a
-   * read is completed.
+   * \param watchdog 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 disable the watchdog using the
+   * Watchdog::off() function to create the watchdog object
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
   bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0,
-                                     const float& read_timeout = 0.2);
+                                     const Watchdog& watchdog = Watchdog::millisec(std::chrono::milliseconds(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 read_timeout Read timeout for the reverse socket on the robot. It is The number of seconds until the read
-   * action times out (float). A timeout of 0 or negative number indicates that the function should not return until a
-   * read is completed.
+   * \param watchdog 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 disable the watchdog using the
+   * Watchdog::off() function to create the watchdog object
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  bool writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action, const float& read_timeout = 0.2);
+  bool writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action,
+                                    const Watchdog& watchdog = Watchdog::millisec(std::chrono::milliseconds(200)));
 
   /*!
    * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot.
@@ -155,6 +159,8 @@ 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_;
 };

include/ur_client_library/ur/ur_driver.h

@@ -212,7 +212,7 @@ class UrDriver
    * \returns True on successful write.
    */
   bool writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode,
-                         const Watchdog& watchdog = Watchdog::sec(0.02));
+                         const Watchdog& watchdog = Watchdog::millisec(std::chrono::milliseconds(20)));
 
   /*!
    * \brief Writes a trajectory point onto the dedicated socket.
@@ -274,7 +274,8 @@ class UrDriver
    * \returns True on successful write.
    */
   bool writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action,
-                                     const int point_number = 0, const Watchdog& watchdog = Watchdog::sec(0.2));
+                                     const int point_number = 0,
+                                     const Watchdog& watchdog = Watchdog::millisec(std::chrono::milliseconds(200)));
 
   /*!
    * \brief Writes a control message in freedrive mode.
@@ -287,7 +288,7 @@ class UrDriver
    * \returns True on successful write.
    */
   bool writeFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action,
-                                    const Watchdog& watchdog = Watchdog::sec(0.2));
+                                    const Watchdog& watchdog = Watchdog::millisec(std::chrono::milliseconds(200)));
 
   /*!
    * \brief Zero the force torque sensor (only availbe on e-Series). Note:  It requires the external control script to
@@ -379,7 +380,7 @@ class UrDriver
    *
    * \returns True on successful write.
    */
-  bool writeKeepalive(const Watchdog& watchdog = Watchdog::sec(1.0));
+  bool writeKeepalive(const Watchdog& watchdog = Watchdog::millisec(std::chrono::milliseconds(1000)));
 
   /*!
    * \brief Starts the RTDE communication.
@@ -488,19 +489,6 @@ class UrDriver
 private:
   static std::string readScriptFile(const std::string& filename);
 
-  /*!
-   * \brief Helper function to verify that the watchdog timeout is configured appropriately given the current control
-   * mode
-   *
-   * \param watchdog watchdog object
-   * \param control_mode current control mode
-   * \param step_time The robots step time
-   *
-   * \returns watchdog timeout
-   */
-  static float verifyWatchdogTimeout(const Watchdog& watchdog, const comm::ControlMode& control_mode,
-                                     const float& step_time);
-
   int rtde_frequency_;
   comm::INotifier notifier_;
   std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
@@ -513,7 +501,7 @@ class UrDriver
 
   uint32_t servoj_gain_;
   double servoj_lookahead_time_;
-  float step_time_;
+  std::chrono::milliseconds step_time_;
 
   std::function<void(bool)> handle_program_state_;
 

include/ur_client_library/ur/watchdog.h

@@ -30,13 +30,16 @@
 
 #pragma once
 
+#include "ur_client_library/comm/control_mode.h"
+#include <chrono>
+
 namespace urcl
 {
 /*!
  * \brief Watchdog class containing a timeout configuration
  *
  * This watchdog is used to configure the read timeout for the reverse socket running in the external control script.
- * The read timeout is the number of seconds until the read action times out (float). A timeout of 0 or negative number
+ * 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 blocking on
  * the robot. This can be set using the function off().
  *
@@ -50,11 +53,11 @@ class Watchdog
   /*!
    * \brief Create a watchdog object with a specific timeout
    *
-   * \param seconds watchdog timeout
+   * \param milliseconds watchdog timeout
    *
    * \returns Watchdog object
    */
-  static Watchdog sec(const float& seconds = 0.02);
+  static Watchdog millisec(const std::chrono::milliseconds& milliseconds = std::chrono::milliseconds(20));
 
   /*!
    * \brief Creates a watchdog object with no timeout
@@ -63,7 +66,19 @@ class Watchdog
    */
   static Watchdog off();
 
-  float timeout_;
+  /*!
+   * \brief Helper function to verify that the watchdog timeout is configured appropriately given the current control
+   * mode
+   *
+   * \param control_mode current control mode
+   * \param step_time The robots step time
+   *
+   * \returns watchdog timeout
+   */
+  std::chrono::milliseconds verifyWatchdogTimeout(const comm::ControlMode& control_mode,
+                                                  const std::chrono::milliseconds& step_time) const;
+
+  std::chrono::milliseconds timeout_;
 };
 
 }  // namespace urcl

resources/external_control.urscript

@@ -515,7 +515,8 @@ while control_mode > MODE_STOPPED:
   enter_critical
   params_mult = socket_read_binary_integer(REVERSE_INTERFACE_DATA_DIMENSION, "reverse_socket", read_timeout)
   if params_mult[0] > 0:
-    read_timeout = params_mult[1] / MULT_jointstate # First element is the read timeout
+    # Convert to read timeout from milliseconds to seconds
+    read_timeout = params_mult[1] / 1000 
     if control_mode != params_mult[REVERSE_INTERFACE_DATA_DIMENSION]:
       # Clear remaining trajectory points
       if control_mode == MODE_FORWARD:

src/control/reverse_interface.cpp

@@ -33,10 +33,12 @@ namespace urcl
 {
 namespace control
 {
-ReverseInterface::ReverseInterface(uint32_t port, std::function<void(bool)> handle_program_state)
+ReverseInterface::ReverseInterface(uint32_t port, std::function<void(bool)> handle_program_state,
+                                   std::chrono::milliseconds step_time)
   : client_fd_(-1)
   , server_(port)
   , handle_program_state_(handle_program_state)
+  , step_time_(step_time)
   , keep_alive_count_modified_deprecated_(false)
 {
   handle_program_state_(false);
@@ -49,7 +51,7 @@ ReverseInterface::ReverseInterface(uint32_t port, std::function<void(bool)> hand
 }
 
 bool ReverseInterface::write(const vector6d_t* positions, const comm::ControlMode control_mode,
-                             const float& read_timeout)
+                             const Watchdog& watchdog)
 {
   const int message_length = 7;
   if (client_fd_ == -1)
@@ -59,16 +61,19 @@ bool ReverseInterface::write(const vector6d_t* positions, const comm::ControlMod
   uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
   uint8_t* b_pos = buffer;
 
+  std::chrono::milliseconds read_timeout =
+      watchdog.verifyWatchdogTimeout(comm::ControlMode::MODE_FREEDRIVE, step_time_);
+
   // This can be removed once we remove the setkeepAliveCount() method
   auto read_timeout_resolved = read_timeout;
   if (keep_alive_count_modified_deprecated_)
   {
-    // Translate keep alive count into read timeout. 0.02 was the "old read timeout"
-    read_timeout_resolved = 0.02 * keepalive_count_;
+    // Translate keep alive count into read timeout. 20 milliseconds was the "old read timeout"
+    read_timeout_resolved = std::chrono::milliseconds(20 * keepalive_count_);
   }
 
   // The first element is always the read timeout.
-  int32_t val = static_cast<int32_t>(round(read_timeout_resolved * MULT_JOINTSTATE));
+  int32_t val = read_timeout_resolved.count();
   val = htobe32(val);
   b_pos += append(b_pos, val);
 
@@ -102,7 +107,7 @@ bool ReverseInterface::write(const vector6d_t* positions, const comm::ControlMod
 }
 
 bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action,
-                                                     const int point_number, const float& read_timeout)
+                                                     const int point_number, const Watchdog& watchdog)
 {
   const int message_length = 3;
   if (client_fd_ == -1)
@@ -112,16 +117,19 @@ bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMess
   uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
   uint8_t* b_pos = buffer;
 
+  std::chrono::milliseconds read_timeout =
+      watchdog.verifyWatchdogTimeout(comm::ControlMode::MODE_FREEDRIVE, step_time_);
+
   // This can be removed once we remove the setkeepAliveCount() method
   auto read_timeout_resolved = read_timeout;
   if (keep_alive_count_modified_deprecated_)
   {
-    // Translate keep alive count into read timeout. 0.02 was the "old read timeout"
-    read_timeout_resolved = 0.02 * keepalive_count_;
+    // Translate keep alive count into read timeout. 20 milliseconds was the "old read timeout"
+    read_timeout_resolved = std::chrono::milliseconds(20 * keepalive_count_);
   }
 
   // The first element is always the read timeout.
-  int32_t val = static_cast<int32_t>(round(read_timeout_resolved * MULT_JOINTSTATE));
+  int32_t val = read_timeout_resolved.count();
   val = htobe32(val);
   b_pos += append(b_pos, val);
 
@@ -147,7 +155,7 @@ bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMess
 }
 
 bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action,
-                                                    const float& read_timeout)
+                                                    const Watchdog& watchdog)
 {
   const int message_length = 2;
   if (client_fd_ == -1)
@@ -157,16 +165,19 @@ bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessag
   uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
   uint8_t* b_pos = buffer;
 
+  std::chrono::milliseconds read_timeout =
+      watchdog.verifyWatchdogTimeout(comm::ControlMode::MODE_FREEDRIVE, step_time_);
+
   // This can be removed once we remove the setkeepAliveCount() method
   auto read_timeout_resolved = read_timeout;
   if (keep_alive_count_modified_deprecated_)
   {
-    // Translate keep alive count into read timeout. 0.02 was the "old read timeout"
-    read_timeout_resolved = 0.02 * keepalive_count_;
+    // Translate keep alive count into read timeout. 20 milliseconds was the "old read timeout"
+    read_timeout_resolved = std::chrono::milliseconds(20 * keepalive_count_);
   }
 
   // The first element is always the read timeout.
-  int32_t val = static_cast<int32_t>(round(read_timeout_resolved * MULT_JOINTSTATE));
+  int32_t val = read_timeout_resolved.count();
   val = htobe32(val);
   b_pos += append(b_pos, val);