Add warnings when calling functions on non-supported PolyScope versions

Author: urfeex

include/ur_client_library/control/reverse_interface.h

@@ -34,6 +34,7 @@
 #include "ur_client_library/types.h"
 #include "ur_client_library/log.h"
 #include "ur_client_library/ur/robot_receive_timeout.h"
+#include "ur_client_library/ur/version_information.h"
 #include <cstring>
 #include <endian.h>
 #include <condition_variable>
@@ -62,6 +63,17 @@ enum class FreedriveControlMessage : int32_t
   FREEDRIVE_START = 1,  ///< Represents command to start freedrive mode.
 };
 
+struct ReverseInterfaceConfig
+{
+  uint32_t port = 50001;  //!< Port the server is started on
+  std::function<void(bool)> handle_program_state = [](bool) {
+    return;
+  };  //!< Function handle to a callback on program state changes.
+  std::chrono::milliseconds step_time = std::chrono::milliseconds(8);  //!< The robots step time
+  uint32_t keepalive_count = 0;                                      //!< Number of allowed timeout reads on the robot.
+  VersionInformation robot_software_version = VersionInformation();  //!< The robot software version.
+};
+
 /*!
  * \brief The ReverseInterface class handles communication to the robot. It starts a server and
  * waits for the robot to connect via its URCaps program.
@@ -82,6 +94,8 @@ class ReverseInterface
   ReverseInterface(uint32_t port, std::function<void(bool)> handle_program_state,
                    std::chrono::milliseconds step_time = std::chrono::milliseconds(8));
 
+  ReverseInterface(const ReverseInterfaceConfig& config);
+
   /*!
    * \brief Disconnects possible clients so the reverse interface object can be safely destroyed.
    */
@@ -167,6 +181,8 @@ class ReverseInterface
   socket_t client_fd_;
   comm::TCPServer server_;
 
+  VersionInformation robot_software_version_;
+
   template <typename T>
   size_t append(uint8_t* buffer, T& val)
   {

include/ur_client_library/control/script_command_interface.h

@@ -31,6 +31,7 @@
 
 #include "ur_client_library/control/reverse_interface.h"
 #include "ur_client_library/ur/tool_communication.h"
+#include "ur_client_library/ur/version_information.h"
 
 namespace urcl
 {
@@ -63,6 +64,11 @@ class ScriptCommandInterface : public ReverseInterface
    */
   ScriptCommandInterface(uint32_t port);
 
+  ScriptCommandInterface(const ReverseInterfaceConfig& config) : ReverseInterface(config)
+  {
+    client_connected_ = false;
+  }
+
   /*!
    * \brief Zero the force torque sensor
    *
@@ -226,6 +232,19 @@ class ScriptCommandInterface : public ReverseInterface
     SET_MAX_JOINT_TORQUES = 9,      ///< Set max joint torques
   };
 
+  void warnIfNotSupported(const VersionInformation& min_polyscope5, const VersionInformation& min_polyscopeX,
+                          const std::string& command_name)
+  {
+    if (robot_software_version_ < min_polyscope5 ||
+        (robot_software_version_.major > 5 && robot_software_version_ < min_polyscopeX))
+    {
+      URCL_LOG_WARN("%s is only available for robots with PolyScope %s / %s or "
+                    "later. This robot's version is %s. This command will have no effect.",
+                    command_name.c_str(), min_polyscope5.toString().c_str(), min_polyscopeX.toString().c_str(),
+                    robot_software_version_.toString().c_str());
+    }
+  }
+
   bool client_connected_;
   static const int MAX_MESSAGE_LENGTH = 28;
 
@@ -235,4 +254,4 @@ class ScriptCommandInterface : public ReverseInterface
 }  // namespace control
 }  // namespace urcl
 
-#endif  // UR_CLIENT_LIBRARY_SCRIPT_COMMAND_INTERFACE_H_INCLUDED
+#endif  // UR_CLIENT_LIBRARY_SCRIPT_COMMAND_INTERFACE_H_INCLUDED

src/control/reverse_interface.cpp

@@ -35,10 +35,16 @@ namespace control
 {
 ReverseInterface::ReverseInterface(uint32_t port, std::function<void(bool)> handle_program_state,
                                    std::chrono::milliseconds step_time)
+  : ReverseInterface(ReverseInterfaceConfig{ port, handle_program_state, step_time })
+{
+}
+
+ReverseInterface::ReverseInterface(const ReverseInterfaceConfig& config)
   : client_fd_(INVALID_SOCKET)
-  , server_(port)
-  , handle_program_state_(handle_program_state)
-  , step_time_(step_time)
+  , server_(config.port)
+  , robot_software_version_(config.robot_software_version)
+  , handle_program_state_(config.handle_program_state)
+  , step_time_(config.step_time)
   , keep_alive_count_modified_deprecated_(false)
 {
   handle_program_state_(false);

src/control/script_command_interface.cpp

@@ -224,6 +224,8 @@ bool ScriptCommandInterface::endToolContact()
 
 bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled)
 {
+  warnIfNotSupported(urcl::VersionInformation::fromString("5.23.0"), urcl::VersionInformation::fromString("10.10.0"),
+                     __func__);
   const int message_length = 2;
   uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
   uint8_t* b_pos = buffer;
@@ -247,6 +249,8 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens
 
 bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd)
 {
+  warnIfNotSupported(urcl::VersionInformation::fromString("5.23.0"), urcl::VersionInformation::fromString("10.10.0"),
+                     __func__);
   const int message_length = 13;
   uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
   uint8_t* b_pos = buffer;
@@ -279,6 +283,8 @@ bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, co
 
 bool ScriptCommandInterface::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques)
 {
+  warnIfNotSupported(urcl::VersionInformation::fromString("5.23.0"), urcl::VersionInformation::fromString("10.10.0"),
+                     __func__);
   const int message_length = 7;
   uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
   uint8_t* b_pos = buffer;
@@ -354,4 +360,4 @@ void ScriptCommandInterface::messageCallback(const socket_t filedescriptor, char
 }
 
 }  // namespace control
-}  // namespace urcl
+}  // namespace urcl

src/ur/ur_driver.cpp

@@ -93,7 +93,10 @@ void UrDriver::init(const UrDriverConfiguration& config)
   std::string local_ip = config.reverse_ip.empty() ? rtde_client_->getIP() : config.reverse_ip;
 
   trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port));
-  script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port));
+  control::ReverseInterfaceConfig script_command_config;
+  script_command_config.port = config.script_command_port;
+  script_command_config.robot_software_version = rtde_client_->getVersion();
+  script_command_interface_.reset(new control::ScriptCommandInterface(script_command_config));
 
   startPrimaryClientCommunication();
 
@@ -720,7 +723,12 @@ void UrDriver::setupReverseInterface(const uint32_t reverse_port)
 {
   auto rtde_frequency = rtde_client_->getTargetFrequency();
   auto step_time = std::chrono::milliseconds(static_cast<int>(1000 / rtde_frequency));
-  reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state_, step_time));
+  control::ReverseInterfaceConfig config;
+  config.step_time = step_time;
+  config.port = reverse_port;
+  config.handle_program_state = handle_program_state_;
+  config.robot_software_version = robot_version_;
+  reverse_interface_.reset(new control::ReverseInterface(config));
 }
 
 void UrDriver::startPrimaryClientCommunication()