Initial tests

Author: VinDp

ur_controllers/CMakeLists.txt

@@ -131,4 +131,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_freedrive_mode_controller
+    test/test_load_freedrive_mode_controller.cpp
+  )
+  target_link_libraries(test_load_freedrive_mode_controller
+    ${PROJECT_NAME}
+  )
+  ament_target_dependencies(test_load_freedrive_mode_controller
+    controller_manager
+    ros2_control_test_assets
+  )
+endif()
+
 ament_package()

ur_controllers/test/freedrive_mode_controller_params.yaml

@@ -0,0 +1,5 @@
+---
+freedrive_mode_controller:
+  ros__parameters:
+    tf_prefix: ""
+    inactive_timeout: 10

ur_controllers/test/test_load_freedrive_mode_controller.cpp

@@ -0,0 +1,60 @@
+// 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(TestLoadFreedriveModeController, 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 } + "/freedrive_mode_controller_params.yaml";
+  cm.set_parameter({ "test_freedrive_mode_controller.params_file", test_file_path });
+
+  cm.set_parameter({ "test_freedrive_mode_controller.type", "ur_controllers/FreedriveModeController" });
+
+  ASSERT_NE(cm.load_controller("test_freedrive_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;
+}

ur_robot_driver/test/integration_test_controller_switch.py

@@ -94,6 +94,7 @@ def test_activating_multiple_controllers_same_interface_fails(self):
                     "forward_position_controller",
                     "forward_velocity_controller",
                     "passthrough_trajectory_controller",
+                    "freedrive_mode_controller",
                 ],
             ).ok
         )
@@ -138,6 +139,7 @@ def test_activating_multiple_controllers_different_interface_fails(self):
                     "forward_position_controller",
                     "forward_velocity_controller",
                     "passthrough_trajectory_controller",
+                    "freedrive_mode_controller",
                 ],
             ).ok
         )
@@ -177,6 +179,15 @@ def test_activating_multiple_controllers_different_interface_fails(self):
                 ],
             ).ok
         )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "freedrive_mode_controller",
+                ],
+            ).ok
+        )
 
     def test_activating_controller_with_running_position_controller_fails(self):
         # Having a position-based controller active, no other controller should be able to
@@ -191,6 +202,7 @@ def test_activating_controller_with_running_position_controller_fails(self):
                     "joint_trajectory_controller",
                     "forward_position_controller",
                     "forward_velocity_controller",
+                    "freedrive_mode_controller",
                     "passthrough_trajectory_controller",
                 ],
             ).ok
@@ -211,6 +223,14 @@ def test_activating_controller_with_running_position_controller_fails(self):
                 ],
             ).ok
         )
+        self.assertFalse(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.STRICT,
+                activate_controllers=[
+                    "freedrive_mode_controller",
+                ],
+            ).ok
+        )
         self.assertFalse(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.STRICT,