Added integration tests for instruction executor

Author: urfeex

examples/CMakeLists.txt

@@ -7,9 +7,8 @@ project(ur_driver_examples)
 # # Check C++11 support / enable global pedantic and Wall
 # #
 include(DefineCXX17CompilerFlag)
-#DEFINE_CXX_17_COMPILER_FLAG(CXX17_FLAG)
-#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20")
+DEFINE_CXX_17_COMPILER_FLAG(CXX17_FLAG)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic")
 
 add_executable(driver_example
   full_driver.cpp)

include/ur_client_library/control/motion_primitives.h

@@ -46,6 +46,7 @@ enum class MotionType : uint8_t
   MOVEP = 2,
   MOVEC = 3,
   SPLINE = 51,
+  UNKNOWN = 255
 };
 struct MotionPrimitive
 {
@@ -91,4 +92,4 @@ struct MoveLPrimitive : public MotionPrimitive
 };
 }  // namespace control
 }  // namespace urcl
-#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
+#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED

include/ur_client_library/ur/instruction_executor.h

@@ -56,7 +56,7 @@ class InstructionExecutor
    *
    * \param motion_sequence The sequence of motion primitives to execute
    */
-  void executeMotion(const std::vector<std::shared_ptr<control::MotionPrimitive>>& motion_sequence);
+  bool executeMotion(const std::vector<std::shared_ptr<control::MotionPrimitive>>& motion_sequence);
 
   /**
    * \brief Move the robot to a joint target.
@@ -96,9 +96,10 @@ class InstructionExecutor
   void trajDoneCallback(const urcl::control::TrajectoryResult& result);
   void trajDisconnectCallback(const int filedescriptor);
   std::shared_ptr<urcl::UrDriver> driver_;
-  bool trajectory_done_ = false;
-  urcl::control::TrajectoryResult result_;
+  std::atomic<bool> trajectory_running_ = false;
+  std::mutex trajectory_result_mutex_;
+  urcl::control::TrajectoryResult trajectory_result_;
 };
 }  // namespace urcl
 
-#endif  // UR_CLIENT_LIBRARY_INSTRUCTION_EXECUTOR_H_INCLUDED
+#endif  // UR_CLIENT_LIBRARY_INSTRUCTION_EXECUTOR_H_INCLUDED

src/ur/instruction_executor.cpp

@@ -31,25 +31,30 @@
 #include "ur_client_library/ur/instruction_executor.h"
 void urcl::InstructionExecutor::trajDoneCallback(const urcl::control::TrajectoryResult& result)
 {
-  result_ = result;
-  trajectory_done_ = true;
+  std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+  trajectory_result_ = result;
+  trajectory_running_ = false;
 }
 void urcl::InstructionExecutor::trajDisconnectCallback(const int filedescriptor)
 {
   URCL_LOG_INFO("Trajectory disconnect");
-  result_ = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE;
-  trajectory_done_ = true;
+  std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+  if (trajectory_running_)
+  {
+    trajectory_result_ = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE;
+    trajectory_running_ = false;
+  }
 }
-void urcl::InstructionExecutor::executeMotion(
+bool urcl::InstructionExecutor::executeMotion(
     const std::vector<std::shared_ptr<control::MotionPrimitive>>& motion_sequence)
 {
-  trajectory_done_ = false;
   if (!driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
                                               motion_sequence.size()))
   {
     URCL_LOG_ERROR("Cannot send trajectory control command. No client connected?");
-    return;
-    ;
+    std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+    trajectory_result_ = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE;
+    return false;
   }
 
   for (const auto& primitive : motion_sequence)
@@ -69,43 +74,38 @@ void urcl::InstructionExecutor::executeMotion(
         urcl::vector6d_t pose_vec = { movel_primitive->target_pose.x,  movel_primitive->target_pose.y,
                                       movel_primitive->target_pose.z,  movel_primitive->target_pose.rx,
                                       movel_primitive->target_pose.ry, movel_primitive->target_pose.rz };
-        if (!driver_->writeTrajectoryPoint(pose_vec, primitive->acceleration, primitive->velocity, true,
-                                           primitive->duration.count(), primitive->blend_radius))
-        {
-          URCL_LOG_ERROR("Cannot send trajectory control command. No client connected?");
-          return;
-        }
+        driver_->writeTrajectoryPoint(pose_vec, primitive->acceleration, primitive->velocity, true,
+                                      primitive->duration.count(), primitive->blend_radius);
         break;
       }
       default:
         URCL_LOG_ERROR("Unsupported motion type");
-        return;
+        std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+        trajectory_result_ = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE;
+        return false;
     }
   }
+  trajectory_running_ = true;
 
-  while (!trajectory_done_)
+  while (trajectory_running_)
   {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    if (!driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP))
-    {
-      URCL_LOG_ERROR("Cannot send trajectory control command. No client connected?");
-      return;
-      ;
-    }
+    driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
   }
-  URCL_LOG_INFO("Trajectory done with result %d", result_);
+  URCL_LOG_INFO("Trajectory done with result %d", trajectory_result_);
+  return true;
 }
 bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const double acceleration, const double velocity,
                                       const double time, const double blend_radius)
 {
   executeMotion({ std::make_shared<control::MoveJPrimitive>(
       target, blend_radius, std::chrono::milliseconds(static_cast<int>(time * 1000)), acceleration, velocity) });
-  return result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS;
+  return trajectory_result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS;
 }
 bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acceleration, const double velocity,
                                       const double time, const double blend_radius)
 {
   executeMotion({ std::make_shared<control::MoveLPrimitive>(
       target, blend_radius, std::chrono::milliseconds(static_cast<int>(time * 1000)), acceleration, velocity) });
-  return result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS;
-}
+  return trajectory_result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS;
+}

tests/CMakeLists.txt

@@ -60,6 +60,13 @@ if (INTEGRATION_TESTS)
   gtest_add_tests(TARGET      ur_driver_tests
                   WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
   )
+
+  add_executable(instruction_executor_test test_instruction_executor.cpp)
+  target_include_directories(instruction_executor_test PRIVATE ${GTEST_INCLUDE_DIRS})
+  target_link_libraries(instruction_executor_test PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+  gtest_add_tests(TARGET instruction_executor_test
+                  WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+  )
 else()
   message(STATUS "Skipping integration tests.")
 endif()
@@ -231,4 +238,4 @@ target_compile_options(control_mode_tests PRIVATE ${CXX17_FLAG})
 target_include_directories(control_mode_tests PRIVATE ${GTEST_INCLUDE_DIRS})
 target_link_libraries(control_mode_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET control_mode_tests
-)
+)