Skip to content

Commit 945b3cf

Browse files
VitezGabrielaVitezGabriela
committed
Add tests for error recovery
1 parent 1e0a7dd commit 945b3cf

File tree

5 files changed

+222
-0
lines changed

5 files changed

+222
-0
lines changed

‎controller_manager/CMakeLists.txt‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,7 @@ if(BUILD_TESTING)
6363
find_package(ament_cmake_gmock REQUIRED)
6464
find_package(ros2_control_test_assets REQUIRED)
6565
find_package(example_interfaces REQUIRED)
66+
find_package(ament_cmake_gtest REQUIRED)
6667

6768
# Plugin Libraries that are built and installed for use in testing
6869
add_library(test_controller SHARED test/test_controller/test_controller.cpp)
@@ -243,6 +244,15 @@ if(BUILD_TESTING)
243244
${controller_manager_msgs_TARGETS}
244245
)
245246

247+
ament_add_gtest(test_controller_manager_with_resource_manager
248+
test/test_controller_manager_with_resource_manager.cpp
249+
)
250+
target_link_libraries(test_controller_manager_with_resource_manager
251+
controller_manager
252+
hardware_interface::hardware_interface
253+
ros2_control_test_assets::ros2_control_test_assets
254+
)
255+
246256
find_package(ament_cmake_pytest REQUIRED)
247257
install(FILES test/test_ros2_control_node.yaml
248258
DESTINATION test)

‎controller_manager/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
<exec_depend>python3-filelock</exec_depend>
3737

3838
<test_depend>ament_cmake_gmock</test_depend>
39+
<test_depend>ament_cmake_gtest</test_depend>
3940
<test_depend>ament_cmake_pytest</test_depend>
4041
<test_depend>hardware_interface_testing</test_depend>
4142
<test_depend>launch_testing_ros</test_depend>

‎controller_manager/test/test_controller_manager_with_resource_manager.cpp‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
#include "test_controller_manager_with_resource_manager.hpp"
2+
3+
std::shared_ptr<rclcpp::Node> ControllerManagerTest::node_ = nullptr;
4+
std::unique_ptr<hardware_interface::ResourceManager> ControllerManagerTest::test_resource_manager_ = nullptr;
5+
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> ControllerManagerTest::executor_ = nullptr;
6+
7+
using LifecycleCallbackReturn =
8+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
9+
10+
void ControllerManagerTest::SetUp()
11+
{
12+
if (!rclcpp::ok()) {
13+
rclcpp::init(0, nullptr);
14+
}
15+
16+
node_ = std::make_shared<rclcpp::Node>("controller_manager_test_node");
17+
auto clock = node_->get_clock();
18+
auto logger = node_->get_logger();
19+
20+
test_resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(clock, logger);
21+
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
22+
}
23+
24+
void ControllerManagerTest::TearDown()
25+
{
26+
node_.reset();
27+
test_resource_manager_.reset();
28+
executor_.reset();
29+
if (rclcpp::ok()) {
30+
rclcpp::shutdown();
31+
}
32+
}
33+
34+
TEST_F(ControllerManagerTest, robot_description_callback_handles_urdf_without_hardware_plugin)
35+
{
36+
TestControllerManager cm(std::move(test_resource_manager_), executor_);
37+
38+
std_msgs::msg::String invalid_urdf_msg;
39+
invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_without_hardware_plugin;
40+
41+
cm.robot_description_callback(invalid_urdf_msg);
42+
43+
EXPECT_FALSE(cm.is_resource_manager_initialized());
44+
45+
EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());
46+
47+
}
48+
49+
TEST_F(ControllerManagerTest, robot_description_callback_handles_invalid_urdf)
50+
{
51+
TestControllerManager cm(std::move(test_resource_manager_), executor_);
52+
53+
std_msgs::msg::String invalid_urdf_msg;
54+
invalid_urdf_msg.data = R"(<robot malformed></robot>)";
55+
56+
cm.robot_description_callback(invalid_urdf_msg);
57+
58+
EXPECT_FALSE(cm.is_resource_manager_initialized());
59+
60+
EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());
61+
62+
}
63+
64+
TEST_F(ControllerManagerTest, robot_description_callback_handles_empty_urdf)
65+
{
66+
TestControllerManager cm(std::move(test_resource_manager_), executor_);
67+
68+
std_msgs::msg::String invalid_urdf_msg;
69+
invalid_urdf_msg.data = "";
70+
71+
cm.robot_description_callback(invalid_urdf_msg);
72+
73+
EXPECT_FALSE(cm.is_resource_manager_initialized());
74+
75+
EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());
76+
}
77+
78+
TEST_F(ControllerManagerTest, robot_description_callback_handles_wrong_plugins)
79+
{
80+
TestControllerManager cm(std::move(test_resource_manager_), executor_);
81+
82+
std_msgs::msg::String invalid_urdf_msg;
83+
invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_with_wrong_plugin;
84+
85+
cm.robot_description_callback(invalid_urdf_msg);
86+
87+
EXPECT_FALSE(cm.is_resource_manager_initialized());
88+
89+
EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());
90+
}
91+
92+
TEST_F(ControllerManagerTest, robot_description_callback_handles_no_geometry)
93+
{
94+
TestControllerManager cm(std::move(test_resource_manager_), executor_);
95+
96+
std_msgs::msg::String invalid_urdf_msg;
97+
invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_no_geometry;
98+
99+
cm.robot_description_callback(invalid_urdf_msg);
100+
101+
EXPECT_FALSE(cm.is_resource_manager_initialized());
102+
103+
EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());
104+
}
105+
106+
TEST_F(ControllerManagerTest, init_controller_manager_with_invalid_urdf)
107+
{
108+
const std::string invalid_urdf = ros2_control_test_assets::invalid_urdf_with_wrong_plugin;
109+
110+
TestControllerManager cm(executor_, invalid_urdf, false, "test_controller_manager", "", rclcpp::NodeOptions{});
111+
112+
EXPECT_FALSE(cm.is_resource_manager_initialized());
113+
114+
EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());
115+
}
116+
117+
int main(int argc, char ** argv)
118+
{
119+
::testing::InitGoogleTest(&argc, argv);
120+
int result = RUN_ALL_TESTS();
121+
return result;
122+
}

‎controller_manager/test/test_controller_manager_with_resource_manager.hpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
#ifndef CONTROLLER_MANAGER_TEST_HPP_
2+
#define CONTROLLER_MANAGER_TEST_HPP_
3+
4+
#include <memory>
5+
#include <string>
6+
7+
#include "gtest/gtest.h"
8+
#include "rclcpp/node.hpp"
9+
#include "std_msgs/msg/string.hpp"
10+
#include "lifecycle_msgs/msg/state.hpp"
11+
#include "controller_manager/controller_manager.hpp"
12+
#include "hardware_interface/resource_manager.hpp"
13+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
14+
#include "ros2_control_test_assets/descriptions.hpp"
15+
16+
class TestControllerManager : public controller_manager::ControllerManager
17+
{
18+
public:
19+
using ControllerManager::ControllerManager;
20+
21+
// Expose callbacks
22+
using ControllerManager::robot_description_callback;
23+
24+
using ControllerManager::is_resource_manager_initialized;
25+
26+
using ControllerManager::resource_manager_;
27+
28+
using ControllerManager::get_robot_description_timer;
29+
};
30+
31+
class ControllerManagerTest : public ::testing::Test
32+
{
33+
protected:
34+
static std::shared_ptr<rclcpp::Node> node_;
35+
static std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;
36+
static std::unique_ptr<hardware_interface::ResourceManager> test_resource_manager_;
37+
virtual void SetUp();
38+
virtual void TearDown();
39+
};
40+
41+
#endif

‎ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2171,6 +2171,54 @@ const auto diff_drive_robot_sdf =
21712171
</sdf>
21722172
)";
21732173

2174+
const auto invalid_urdf_without_hardware_plugin =
2175+
R"(
2176+
<robot name="minimal_robot">
2177+
<link name="base_link"/>
2178+
<joint name="joint1" type="revolute">
2179+
<parent link="base_link"/>
2180+
<child link="link1"/>
2181+
<origin xyz="0 0 0"/>
2182+
<axis xyz="0 0 1"/>
2183+
<limit effort="10" lower="-1.57" upper="1.57" velocity="1.0"/>
2184+
</joint>
2185+
<link name="link1"/>
2186+
<ros2_control name="default" type="system">
2187+
</ros2_control>
2188+
</robot>
2189+
)";
2190+
2191+
const auto invalid_urdf_with_wrong_plugin =
2192+
R"(
2193+
<robot name="minimal_robot">
2194+
<link name="base_link"/>
2195+
<joint name="joint1" type="revolute">
2196+
<parent link="base_link"/>
2197+
<child link="link1"/>
2198+
<origin xyz="0 0 0"/>
2199+
<axis xyz="0 0 1"/>
2200+
<limit effort="10" lower="-1.57" upper="1.57" velocity="1.0"/>
2201+
</joint>
2202+
<link name="link1"/>
2203+
<ros2_control name="default" type="system">
2204+
<hardware>
2205+
<plugin>mock_components/NonExistentSystem</plugin>
2206+
</hardware>
2207+
</ros2_control>
2208+
</robot>
2209+
)";
2210+
2211+
const auto invalid_urdf_no_geometry =
2212+
R"(
2213+
<robot name="minimal_robot">
2214+
<ros2_control name="default" type="system">
2215+
<hardware>
2216+
<plugin>mock_components/NonExistentSystem</plugin>
2217+
</hardware>
2218+
</ros2_control>
2219+
</robot>
2220+
)";
2221+
21742222
const auto minimal_robot_urdf =
21752223
std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail);
21762224
const auto minimal_robot_urdf_no_limits = std::string(urdf_head_continuous_missing_limits) +

0 commit comments

Comments
 (0)