Merge remote-tracking branch 'origin/master' into more_motion_target_interfaces

Author: urfeex

.github/workflows/ci.yml

@@ -67,6 +67,7 @@ jobs:
           -DWITH_ASAN=ON
           -DPRIMARY_CLIENT_STRICT_PARSING=ON
           -DCMAKE_COMPILE_WARNING_AS_ERROR=ON
+          -DCHECK_RTDE_DOCS_RECIPE=ON
         env:
           CXXFLAGS: -g -O2 -fprofile-arcs -ftest-coverage
           CFLAGS: -g -O2 -fprofile-arcs -ftest-coverage

include/ur_client_library/primary/primary_client.h

@@ -258,6 +258,20 @@ class PrimaryClient
     return consumer_->getConfigurationData();
   }
 
+  /*!
+   * \brief Get the latest kinematics info.
+   *
+   * The kinematics info contains the controller's calibrated DH parameters: the nominal model
+   * combined with the per-arm calibration deltas, plus the calibration checksum/status. It
+   * will be updated in the background. This will always show the latest received kinematics
+   * info independent of the time that has passed since receiving it. If no kinematics info
+   * has been received yet, this will return a nullptr.
+   */
+  std::shared_ptr<KinematicsInfo> getKinematicsInfo()
+  {
+    return consumer_->getKinematicsInfo();
+  }
+
   /*!
    * \brief Get the Robot type
    *

include/ur_client_library/primary/primary_consumer.h

@@ -107,6 +107,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
   virtual bool consume(KinematicsInfo& pkg) override
   {
     URCL_LOG_DEBUG("%s", pkg.toString().c_str());
+    std::scoped_lock lock(kinematics_info_mutex_);
     kinematics_info_ = std::make_shared<KinematicsInfo>(pkg);
     return true;
   }
@@ -203,6 +204,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
    */
   std::shared_ptr<KinematicsInfo> getKinematicsInfo()
   {
+    std::scoped_lock lock(kinematics_info_mutex_);
     return kinematics_info_;
   }
 
@@ -259,6 +261,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
 private:
   std::function<void(ErrorCode&)> error_code_message_callback_;
   std::shared_ptr<KinematicsInfo> kinematics_info_;
+  std::mutex kinematics_info_mutex_;
   std::mutex robot_mode_mutex_;
   std::shared_ptr<RobotModeData> robot_mode_;
   std::mutex version_information_mutex_;

tests/CMakeLists.txt

@@ -21,23 +21,27 @@ include(GoogleTest)
 
 option(INTEGRATION_TESTS "Build the integration tests that require a running robot / URSim" OFF)
 option(POLYSCOPE_X_TESTS_WITH_REMOTE_CONTROL "Run Polyscope X tests that require remote control" OFF)
+option(CHECK_RTDE_DOCS_RECIPE "Fetch the RTDE documentation to auto-generate a recipe containing all output fields and check that with the RTDE client. Requires python3 and pandas and an internet connection." OFF)
 # Build Tests
 if (INTEGRATION_TESTS)
   # Integration tests require a robot reachable at 192.168.56.101. Therefore, they have to be
   # activated separately.
 
-  find_package(Python3 COMPONENTS Interpreter REQUIRED)
   if(POLYSCOPE_X_TESTS_WITH_REMOTE_CONTROL)
     add_compile_definitions(POLYSCOPE_X_TESTS_WITH_REMOTE_CONTROL=1 )
   endif()
 
-  add_custom_target(generate_outputs ALL COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/resources/generate_rtde_outputs.py)
   add_executable(rtde_tests test_rtde_client.cpp fake_rtde_server.cpp)
-  add_dependencies(rtde_tests generate_outputs)
   target_link_libraries(rtde_tests PRIVATE ur_client_library::urcl GTest::gtest_main)
   gtest_add_tests(TARGET      rtde_tests
                   WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
   )
+  if (CHECK_RTDE_DOCS_RECIPE)
+    find_package(Python3 COMPONENTS Interpreter REQUIRED)
+    add_custom_target(generate_outputs ALL COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/resources/generate_rtde_outputs.py)
+    add_dependencies(rtde_tests generate_outputs)
+    target_compile_definitions(rtde_tests PRIVATE CHECK_RTDE_DOCS_RECIPE=1)
+  endif()
 
   add_executable(dashboard_client_g5_tests test_dashboard_client_g5.cpp)
   target_link_libraries(dashboard_client_g5_tests PRIVATE ur_client_library::urcl GTest::gtest_main)

tests/test_primary_client.cpp

@@ -267,6 +267,24 @@ TEST_F(PrimaryClientTest, test_configuration_data)
   EXPECT_NE(client_->getRobotType(), RobotType::UNDEFINED);
 }
 
+TEST_F(PrimaryClientTest, test_kinematics_info)
+{
+  EXPECT_NO_THROW(client_->start());
+
+  // Once we connect to the primary client we should receive kinematics info
+  auto start_time = std::chrono::system_clock::now();
+  const auto timeout = std::chrono::seconds(1);
+  auto kinematics_info = client_->getKinematicsInfo();
+  while (kinematics_info == nullptr && std::chrono::system_clock::now() - start_time < timeout)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    kinematics_info = client_->getKinematicsInfo();
+  }
+
+  // We should have received a kinematics info package
+  EXPECT_NE(kinematics_info, nullptr);
+}
+
 TEST_F(PrimaryClientTest, test_get_robot_series)
 {
   EXPECT_NO_THROW(client_->start());

tests/test_rtde_client.cpp

@@ -680,6 +680,7 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist)
   client_->pause();
 }
 
+#ifdef CHECK_RTDE_DOCS_RECIPE
 TEST_F(RTDEClientTest, check_rtde_data_fields_match_docs)
 {
   std::ifstream docs_file(docs_output_recipe_file_);
@@ -724,6 +725,7 @@ TEST_F(RTDEClientTest, check_rtde_data_fields_match_docs)
     GTEST_FAIL();
   }
 }
+#endif
 
 TEST_F(RTDEClientTest, check_unknown_rtde_output_variable)
 {