Added first test for force_mode_controller

Author: urfeex

ur_controllers/CMakeLists.txt

@@ -122,4 +122,23 @@ ament_export_libraries(
   ${PROJECT_NAME}
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_force_mode_controller
+    test/test_load_force_mode_controller.cpp
+  )
+  target_link_libraries(test_load_force_mode_controller
+    ${PROJECT_NAME}
+  )
+  ament_target_dependencies(test_load_force_mode_controller
+    controller_manager
+    ros2_control_test_assets
+  )
+endif()
+
 ament_package()

ur_controllers/test/force_mode_controller_params.yaml

@@ -0,0 +1,7 @@
+---
+force_mode_controller:
+  ros__parameters:
+    tf_prefix: ""
+    damping: 0.025
+    gain_scaling: 0.5
+    check_io_successful_retries: 10

ur_controllers/test/test_load_force_mode_controller.cpp

@@ -0,0 +1,62 @@
+// Copyright 2024, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gmock/gmock.h>
+#include "controller_manager/controller_manager.hpp"
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/utilities.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+TEST(TestLoadForceModeController, load_controller)
+{
+  std::shared_ptr<rclcpp::Executor> executor =
+    std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  controller_manager::ControllerManager cm{
+    executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"};
+
+  const std::string test_file_path =
+    std::string{TEST_FILES_DIRECTORY} + "/force_mode_controller_params.yaml";
+  cm.set_parameter({"test_force_mode_controller.params_file", test_file_path});
+
+  cm.set_parameter({"test_force_mode_controller.type", "ur_controllers/ForceModeController"});
+
+  ASSERT_NE(cm.load_controller("test_force_mode_controller"), nullptr);
+}
+
+int main(int argc, char * argv[])
+{
+  ::testing::InitGoogleMock(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+
+  return result;
+}