Merge current master and fix file handling

CHANGELOG.rst

@@ -2,6 +2,43 @@
 Changelog for package ur_client_library
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.7.0 (2025-02-19)
+------------------
+* Make UrDriver tests run without ctest (`#270 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/270>`_)
+* UrDriver: Send program in headless mode after creating trajectory and script_command servers (`#271 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/271>`_)
+* Improve limit check (`#256 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/256>`_)
+* Use colored log output and timestamps in default log handler (`#267 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/267>`_)
+* Parametrize reconnection time for UrDriver (`#266 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/266>`_)
+  Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com>
+* Fix DashboardClient load program from subdir (`#269 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/269>`_)
+* Increase dashboard timeout in ExampleRobotWrapper to 10s
+* Disable internal deprecation warning
+* Use a config struct for initializing UrDriver (`#264 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/pull/264>`_)
+* Use ExampleRobotWrapper for initialization in all examples (`#265 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/265>`_)
+* Enable nightly CI jobs (`#263 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/263>`_)
+* Expose diagnostic error codes (`#225 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/225>`_)
+* RTDEClient: pause and stop in destructor only if running (`#257 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/257>`_)
+* Use coverage flags to distinguish between runs (`#261 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/261>`_)
+* Fix branch name for integration tests run on push (`#262 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/262>`_)
+* Add codecov/test-results-action (`#260 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/260>`_)
+* Fix GH edit URL for trajectory_point_interface example (`#259 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/259>`_)
+* Update URL check
+* Show which example is running in run_examples.sh
+* Add documentation for all examples (`#258 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/pull/258>`_)
+* Update RT setup documentation to point to urcl docs
+* Use EXPECT_NEAR vs EXPECT_EQ
+* Fix typo in start_ursim.sh help
+* Make CI capable to run with urcap
+* Use ExampleRobotWrapper in integration tests (`#252 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/pull/252>`_)
+* Add a wrapper to handle all robot setup (`#252 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/pull/252>`_)
+* Allow clang-format to indent preprocessor directives (`#246 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/246>`_)
+* docs: Clarify that the motion functions use script functions for execution (`#255 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/255>`_)
+* Update link to sphinx-doc.org using https (`#247 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/247>`_)
+* Use joint speed for extrapolation rather than differences (`#254 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/254>`_)
+* Move setup instructions to ur_client_library (`#248 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/248>`_)
+* Add more information about acceleration/velocity parametrization in trajectory examples (`#251 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/251>`_)
+* Contributors: Felix Exner, Rune Søe-Knudsen, jessica-chen-ocado, Dominic Reber
+
 1.6.0 (2025-01-23)
 ------------------
 * Do not throw exception in DashboardClient::sendRequest (`#249 <https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/249>`_)

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ur_client_library</name>
-  <version>1.6.0</version>
+  <version>1.7.0</version>
   <description>Standalone C++ library for accessing Universal Robots interfaces. This has been forked off the ur_robot_driver.</description>
   <author>Thomas Timm Andersen</author>
   <author>Simon Rasmussen</author>

resources/external_control.urscript

@@ -99,16 +99,23 @@ def targetWithinLimits(step_start, step_end, time):
   while idx < 6:
     local velocity = norm(step_end[idx] - step_start[idx]) / time
     if velocity > JOINT_IGNORE_SPEED:
-      local str = str_cat(str_cat("Velocity ", velocity), str_cat(str_cat(" required to reach the received target ", step_end), str_cat(str_cat(" within ", time), " seconds is exceeding the joint velocity limits. Ignoring commands until a valid command is received.")))
+      local str = str_cat(
+        str_cat("Velocity ", velocity), str_cat(
+          str_cat(" required in joint ", idx), str_cat(
+            str_cat(" to go from ", step_start[idx]), str_cat(
+              str_cat(" to ", step_end[idx]), str_cat(
+                str_cat(" within ", time), " seconds is exceeding the joint velocity limits. Ignoring commands until a valid command is received.")))))
       if violation_popup_counter == 0:
         # We want a popup when an invalid commant is sent. As long as we keep sending invalid
         # commands, we do not want to repeat the popup.
-        popup(str, title="External Control error", blocking=False, error=True)
+        popup(str, title="External Control speed limit", blocking=False, warning=True)
       end
       if violation_popup_counter * get_steptime() % 5.0 == 0:
         # We want to have a log printed regularly. We are receiving motion commands that are not
         # feasible. The user should have a chance to know about this.
         textmsg(str)
+        textmsg("start configuration: ", step_start)
+        textmsg("end   configuration: ", step_end)
       end
       violation_popup_counter = violation_popup_counter + 1
       return False
@@ -143,8 +150,10 @@ end
 
 def set_servo_setpoint(q):
   cmd_servo_state = SERVO_RUNNING
-  cmd_servo_q_last = cmd_servo_q
-  cmd_servo_q = q
+  if targetWithinLimits(cmd_servo_q, q, steptime):
+    cmd_servo_q_last = cmd_servo_q
+    cmd_servo_q = q
+  end
 end
 
 def extrapolate():
@@ -156,7 +165,6 @@ end
 thread servoThread():
   textmsg("ExternalControl: Starting servo thread")
   state = SERVO_IDLE
-  local q_last = get_joint_positions()
   while control_mode == MODE_SERVOJ:
     enter_critical
     q = cmd_servo_q
@@ -176,17 +184,11 @@ thread servoThread():
       end
 
       q = extrapolate()
-      if targetWithinLimits(q_last, q, steptime):
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-        q_last = q
-      end
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
 
     elif state == SERVO_RUNNING:
       extrapolate_count = 0
-      if targetWithinLimits(q_last, q, steptime):
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-        q_last = q
-      end
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
     else:
       extrapolate_count = 0
       sync()
@@ -555,7 +557,6 @@ end
 thread servoThreadP():
   textmsg("Starting pose servo thread")
   state = SERVO_IDLE
-  local q_last = get_joint_positions()
   while control_mode == MODE_POSE:
     enter_critical
     q = cmd_servo_q
@@ -575,15 +576,11 @@ thread servoThreadP():
       end
 
       q = extrapolate()
-      if targetWithinLimits(q_last, q, steptime):
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-      end
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
 
     elif state == SERVO_RUNNING:
       extrapolate_count = 0
-      if targetWithinLimits(q_last, q, steptime):
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-      end
+      servoj(q, t=steptime, {{SERVO_J_REPLACE}})
     else:
       extrapolate_count = 0
       sync()
@@ -594,12 +591,6 @@ thread servoThreadP():
   stopj(STOPJ_ACCELERATION)
 end
 
-def set_servo_pose(pose):
-  cmd_servo_state = SERVO_RUNNING
-  cmd_servo_q_last = cmd_servo_q
-  cmd_servo_q = get_inverse_kin(pose, cmd_servo_q)
-end
-
 def tool_contact_detection():
   # Detect tool contact in the directions that the TCP is moving
   step_back = tool_contact(direction = get_actual_tcp_speed())
@@ -718,6 +709,7 @@ while control_mode > MODE_STOPPED:
         join thread_move
       end
       if control_mode == MODE_SERVOJ:
+        cmd_servo_q = get_joint_positions()
         thread_move = run servoThread()
       elif control_mode == MODE_SPEEDJ:
         thread_move = run speedThread()
@@ -727,6 +719,7 @@ while control_mode > MODE_STOPPED:
       elif control_mode == MODE_SPEEDL:
         thread_move = run speedlThread()
       elif control_mode == MODE_POSE:
+        cmd_servo_q = get_joint_positions()
         thread_move = run servoThreadP()
       end
     end
@@ -756,7 +749,7 @@ while control_mode > MODE_STOPPED:
       set_speedl(twist)
     elif control_mode == MODE_POSE:
       pose = p[params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
-      set_servo_pose(pose)
+      set_servo_setpoint(get_inverse_kin(pose, cmd_servo_q))
     elif control_mode == MODE_FREEDRIVE:
       if params_mult[2] == FREEDRIVE_MODE_START:
         textmsg("Entering freedrive mode")

src/default_log_handler.cpp

@@ -30,29 +30,34 @@
 
 #include "ur_client_library/default_log_handler.h"
 #include <stdio.h>
+#include <chrono>
+#include <string>
 
 namespace urcl
 {
+
 DefaultLogHandler::DefaultLogHandler() = default;
 
 void DefaultLogHandler::log(const char* file, int line, LogLevel loglevel, const char* log)
 {
+  auto timestamp = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch());
+
   switch (loglevel)
   {
     case LogLevel::INFO:
-      printf("%s%s %i: %s \n", "INFO ", file, line, log);
+      printf("[%f] %s%s %i: %s \n", timestamp.count(), "INFO ", file, line, log);
       break;
     case LogLevel::DEBUG:
-      printf("%s%s %i: %s \n", "DEBUG ", file, line, log);
+      printf("\033[36m[%f] %s%s %i: %s \033[0m\n", timestamp.count(), "DEBUG ", file, line, log);
       break;
     case LogLevel::WARN:
-      printf("%s%s %i: %s \n", "WARN ", file, line, log);
+      printf("\033[33m[%f] %s%s %i: %s \033[0m\n", timestamp.count(), "WARN ", file, line, log);
       break;
     case LogLevel::ERROR:
-      printf("%s%s %i: %s \n", "ERROR ", file, line, log);
+      printf("\033[31m[%f] %s%s %i: %s \033[0m\n", timestamp.count(), "ERROR ", file, line, log);
       break;
     case LogLevel::FATAL:
-      printf("%s%s %i: %s \n", "FATAL ", file, line, log);
+      printf("\033[31m[%f] %s%s %i: %s \033[0m\n", timestamp.count(), "FATAL ", file, line, log);
       break;
     default:
       break;

src/ur/ur_driver.cpp

@@ -148,6 +148,9 @@ void UrDriver::init(const UrDriverConfiguration& config)
   }
   prog.replace(prog.find(BEGIN_REPLACE), BEGIN_REPLACE.length(), begin_replace.str());
 
+  trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port));
+  script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port));
+
   if (in_headless_mode_)
   {
     full_robot_program_ = "stop program\n";
@@ -167,9 +170,6 @@ void UrDriver::init(const UrDriverConfiguration& config)
     URCL_LOG_DEBUG("Created script sender");
   }
 
-  trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port));
-  script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port));
-
   if (!std::empty(config.calibration_checksum))
   {
     URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been "

tests/test_ur_driver.cpp

@@ -59,7 +59,17 @@ class UrDriverTest : public ::testing::Test
 
   void SetUp()
   {
-    ASSERT_TRUE(g_my_robot->isHealthy());
+    if (!g_my_robot->isHealthy())
+    {
+      ASSERT_TRUE(g_my_robot->resendRobotProgram());
+      ASSERT_TRUE(g_my_robot->waitForProgramRunning(500));
+    }
+  }
+
+  void TearDown()
+  {
+    g_my_robot->ur_driver_->stopControl();
+    g_my_robot->waitForProgramNotRunning(1000);
   }
 };
 
@@ -71,21 +81,24 @@ TEST_F(UrDriverTest, read_non_existing_script_file)
 
 TEST_F(UrDriverTest, read_existing_script_file)
 {
+  int fd = 0;
+  char existing_script_file[] = "urscript.XXXXXX";
 #ifdef _WIN32
 #  define mkstemp _mktemp_s
 #endif
-  char existing_script_file[] = "urscript.XXXXXX";
-  int fd = mkstemp(existing_script_file);
-  if (fd == -1)
+  mkstemp(existing_script_file);
+
+  std::ofstream ofs(existing_script_file);
+  if (ofs.bad())
   {
     std::cout << "Failed to create temporary files" << std::endl;
     GTEST_FAIL();
   }
   EXPECT_NO_THROW(UrDriver::readScriptFile(existing_script_file));
 
   // clean up
-  close(fd);
-  unlink(existing_script_file);
+  ofs.close();
+  std::remove(existing_script_file);
 }
 
 TEST_F(UrDriverTest, robot_receive_timeout)
@@ -224,13 +237,16 @@ TEST_F(UrDriverTest, send_robot_program_retry_on_failure)
 {
   // Check that sendRobotProgram is robust to the secondary stream being disconnected. This is what happens when
   // switching from Remote to Local and back to Remote mode for example.
-  g_my_robot->ur_driver_->closeSecondaryStream();
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  // To be able to re-send the robot program we'll have to make sure it isn't running
+  g_my_robot->ur_driver_->stopControl();
+  g_my_robot->waitForProgramNotRunning();
+
+  g_my_robot->ur_driver_->closeSecondaryStream();
 
   EXPECT_TRUE(g_my_robot->resendRobotProgram());
 
-  EXPECT_TRUE(g_my_robot->waitForProgramRunning());
+  EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000));
 }
 
 TEST_F(UrDriverTest, reset_rtde_client)