Skip to content

Commit 54f1967

Browse files
saikishorsaikishor
committed
Add tests for the handle_exceptions parameter
1 parent de6f40f commit 54f1967

7 files changed

+292
-9
lines changed

‎controller_manager/test/controller_manager_test_common.hpp‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -57,14 +57,19 @@ class ControllerManagerFixture : public ::testing::Test
5757
public:
5858
explicit ControllerManagerFixture(
5959
const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf,
60-
const std::string & cm_namespace = "")
60+
const std::string & cm_namespace = "", std::vector<rclcpp::Parameter> cm_parameters = {})
6161
: robot_description_(robot_description)
6262
{
6363
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
64+
rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options();
65+
if (!cm_parameters.empty())
66+
{
67+
cm_node_options.parameter_overrides(cm_parameters);
68+
}
6469
cm_ = std::make_shared<CtrlMgr>(
6570
std::make_unique<hardware_interface::ResourceManager>(
6671
rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
67-
executor_, TEST_CM_NAME, cm_namespace);
72+
executor_, TEST_CM_NAME, cm_namespace, cm_node_options);
6873
// We want to be able to not pass robot description immediately
6974
if (!robot_description_.empty())
7075
{

‎controller_manager/test/test_controller/test_controller.cpp‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,10 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
6161
controller_interface::return_type TestController::update(
6262
const rclcpp::Time & time, const rclcpp::Duration & period)
6363
{
64+
if (throw_on_update)
65+
{
66+
throw std::runtime_error("Exception from TestController::update() as requested.");
67+
}
6468
if (time.get_clock_type() != RCL_ROS_TIME)
6569
{
6670
throw std::runtime_error("ROS Time is required for the controller to operate.");
@@ -101,7 +105,14 @@ controller_interface::return_type TestController::update(
101105
return controller_interface::return_type::OK;
102106
}
103107

104-
CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; }
108+
CallbackReturn TestController::on_init()
109+
{
110+
if (throw_on_initialize)
111+
{
112+
throw std::runtime_error("Exception from TestController::on_init() as requested.");
113+
}
114+
return CallbackReturn::SUCCESS;
115+
}
105116

106117
CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
107118
{

‎controller_manager/test/test_controller/test_controller.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,9 @@ class TestController : public controller_interface::ControllerInterface
8383
// errors
8484
double set_first_command_interface_value_to;
8585
rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.);
86+
87+
bool throw_on_initialize = false;
88+
bool throw_on_update = false;
8689
};
8790

8891
} // namespace test_controller

‎controller_manager/test/test_controller_manager.cpp‎

Lines changed: 261 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3037,3 +3037,264 @@ TEST_F(
30373037
test_controller->get_lifecycle_state().id());
30383038
}
30393039
}
3040+
3041+
class TestControllerManagerWithHandlingExceptions
3042+
: public ControllerManagerFixture<controller_manager::ControllerManager>
3043+
{
3044+
};
3045+
3046+
TEST_F(TestControllerManagerWithHandlingExceptions, controller_lifecycle_on_exceptions)
3047+
{
3048+
auto test_controller = std::make_shared<test_controller::TestController>();
3049+
3050+
test_controller->throw_on_initialize = true;
3051+
3052+
EXPECT_NO_THROW(cm_->add_controller(
3053+
test_controller, test_controller::TEST_CONTROLLER_NAME,
3054+
test_controller::TEST_CONTROLLER_CLASS_NAME));
3055+
EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
3056+
3057+
test_controller->throw_on_initialize = false;
3058+
cm_->add_controller(
3059+
test_controller, test_controller::TEST_CONTROLLER_NAME,
3060+
test_controller::TEST_CONTROLLER_CLASS_NAME);
3061+
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
3062+
EXPECT_EQ(2, test_controller.use_count());
3063+
3064+
// setup interface to claim from controllers
3065+
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
3066+
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
3067+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
3068+
{
3069+
cmd_itfs_cfg.names.push_back(interface);
3070+
}
3071+
test_controller->set_command_interface_configuration(cmd_itfs_cfg);
3072+
3073+
controller_interface::InterfaceConfiguration state_itfs_cfg;
3074+
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
3075+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
3076+
{
3077+
state_itfs_cfg.names.push_back(interface);
3078+
}
3079+
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
3080+
{
3081+
state_itfs_cfg.names.push_back(interface);
3082+
}
3083+
test_controller->set_state_interface_configuration(state_itfs_cfg);
3084+
3085+
EXPECT_EQ(
3086+
controller_interface::return_type::OK,
3087+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
3088+
EXPECT_EQ(0u, test_controller->internal_counter)
3089+
<< "Update should not reach an unconfigured controller";
3090+
3091+
EXPECT_EQ(
3092+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
3093+
test_controller->get_lifecycle_state().id());
3094+
3095+
// configure controller
3096+
{
3097+
ControllerManagerRunner cm_runner(this);
3098+
EXPECT_EQ(
3099+
controller_interface::return_type::OK,
3100+
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME));
3101+
}
3102+
3103+
EXPECT_EQ(
3104+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
3105+
test_controller->get_lifecycle_state().id());
3106+
3107+
std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
3108+
std::vector<std::string> stop_controllers = {};
3109+
{
3110+
ControllerManagerRunner cm_runner(this);
3111+
auto switch_future = std::async(
3112+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
3113+
start_controllers, stop_controllers,
3114+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
3115+
rclcpp::Duration(0, 0));
3116+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
3117+
<< "switch_controller should be blocking until next update cycle";
3118+
3119+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
3120+
}
3121+
EXPECT_EQ(
3122+
controller_interface::return_type::OK,
3123+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
3124+
3125+
EXPECT_EQ(
3126+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
3127+
3128+
test_controller->throw_on_update = true;
3129+
EXPECT_NO_THROW(cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
3130+
3131+
EXPECT_EQ(
3132+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
3133+
test_controller->get_lifecycle_state().id());
3134+
3135+
test_controller->throw_on_update = false;
3136+
{
3137+
ControllerManagerRunner cm_runner(this);
3138+
auto switch_future = std::async(
3139+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
3140+
start_controllers, stop_controllers,
3141+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
3142+
rclcpp::Duration(0, 0));
3143+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
3144+
<< "switch_controller should be blocking until next update cycle";
3145+
3146+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
3147+
}
3148+
EXPECT_EQ(
3149+
controller_interface::return_type::OK,
3150+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
3151+
3152+
EXPECT_EQ(
3153+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
3154+
3155+
EXPECT_EQ(
3156+
controller_interface::return_type::OK,
3157+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
3158+
3159+
{
3160+
ControllerManagerRunner cm_runner(this);
3161+
auto switch_future = std::async(
3162+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
3163+
stop_controllers, start_controllers,
3164+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
3165+
rclcpp::Duration(0, 0));
3166+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
3167+
<< "switch_controller should be blocking until next update cycle";
3168+
3169+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
3170+
}
3171+
3172+
EXPECT_EQ(
3173+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
3174+
test_controller->get_lifecycle_state().id());
3175+
}
3176+
3177+
class TestControllerManagerNotHandlingExceptions
3178+
: public ControllerManagerFixture<controller_manager::ControllerManager>
3179+
{
3180+
public:
3181+
TestControllerManagerNotHandlingExceptions()
3182+
: ControllerManagerFixture<controller_manager::ControllerManager>(
3183+
ros2_control_test_assets::minimal_robot_urdf, "",
3184+
{rclcpp::Parameter("handle_exceptions", false)})
3185+
{
3186+
}
3187+
};
3188+
3189+
TEST_F(TestControllerManagerNotHandlingExceptions, controller_lifecycle_on_exceptions)
3190+
{
3191+
auto test_controller = std::make_shared<test_controller::TestController>();
3192+
3193+
test_controller->throw_on_initialize = true;
3194+
3195+
EXPECT_THROW(
3196+
cm_->add_controller(
3197+
test_controller, test_controller::TEST_CONTROLLER_NAME,
3198+
test_controller::TEST_CONTROLLER_CLASS_NAME),
3199+
std::runtime_error);
3200+
EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
3201+
3202+
test_controller->throw_on_initialize = false;
3203+
cm_->add_controller(
3204+
test_controller, test_controller::TEST_CONTROLLER_NAME,
3205+
test_controller::TEST_CONTROLLER_CLASS_NAME);
3206+
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
3207+
EXPECT_EQ(2, test_controller.use_count());
3208+
3209+
// setup interface to claim from controllers
3210+
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
3211+
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
3212+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
3213+
{
3214+
cmd_itfs_cfg.names.push_back(interface);
3215+
}
3216+
test_controller->set_command_interface_configuration(cmd_itfs_cfg);
3217+
3218+
controller_interface::InterfaceConfiguration state_itfs_cfg;
3219+
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
3220+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
3221+
{
3222+
state_itfs_cfg.names.push_back(interface);
3223+
}
3224+
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
3225+
{
3226+
state_itfs_cfg.names.push_back(interface);
3227+
}
3228+
test_controller->set_state_interface_configuration(state_itfs_cfg);
3229+
3230+
EXPECT_EQ(
3231+
controller_interface::return_type::OK,
3232+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
3233+
EXPECT_EQ(0u, test_controller->internal_counter)
3234+
<< "Update should not reach an unconfigured controller";
3235+
3236+
EXPECT_EQ(
3237+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
3238+
test_controller->get_lifecycle_state().id());
3239+
3240+
// configure controller
3241+
{
3242+
ControllerManagerRunner cm_runner(this);
3243+
EXPECT_EQ(
3244+
controller_interface::return_type::OK,
3245+
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME));
3246+
}
3247+
3248+
EXPECT_EQ(
3249+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
3250+
test_controller->get_lifecycle_state().id());
3251+
3252+
std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
3253+
std::vector<std::string> stop_controllers = {};
3254+
{
3255+
ControllerManagerRunner cm_runner(this);
3256+
auto switch_future = std::async(
3257+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
3258+
start_controllers, stop_controllers,
3259+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
3260+
rclcpp::Duration(0, 0));
3261+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
3262+
<< "switch_controller should be blocking until next update cycle";
3263+
3264+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
3265+
}
3266+
EXPECT_EQ(
3267+
controller_interface::return_type::OK,
3268+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
3269+
3270+
EXPECT_EQ(
3271+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
3272+
3273+
test_controller->throw_on_update = true;
3274+
EXPECT_THROW(cm_->update(time_, rclcpp::Duration::from_seconds(0.01)), std::runtime_error);
3275+
3276+
test_controller->throw_on_update = false;
3277+
EXPECT_EQ(
3278+
controller_interface::return_type::OK,
3279+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
3280+
3281+
EXPECT_EQ(
3282+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
3283+
3284+
{
3285+
ControllerManagerRunner cm_runner(this);
3286+
auto switch_future = std::async(
3287+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
3288+
stop_controllers, start_controllers,
3289+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
3290+
rclcpp::Duration(0, 0));
3291+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
3292+
<< "switch_controller should be blocking until next update cycle";
3293+
3294+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
3295+
}
3296+
3297+
EXPECT_EQ(
3298+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
3299+
test_controller->get_lifecycle_state().id());
3300+
}

‎controller_manager/test/test_controller_manager_hardware_error_handling.cpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -55,9 +55,10 @@ class TestableControllerManager : public controller_manager::ControllerManager
5555
std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
5656
std::shared_ptr<rclcpp::Executor> executor,
5757
const std::string & manager_node_name = "controller_manager",
58-
const std::string & node_namespace = "")
58+
const std::string & node_namespace = "",
59+
const rclcpp::NodeOptions & node_options = controller_manager::get_cm_node_options())
5960
: controller_manager::ControllerManager(
60-
std::move(resource_manager), executor, manager_node_name, node_namespace)
61+
std::move(resource_manager), executor, manager_node_name, node_namespace, node_options)
6162
{
6263
}
6364
};

‎controller_manager/test/test_controller_manager_urdf_passing.cpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,9 +46,10 @@ class TestableControllerManager : public controller_manager::ControllerManager
4646
std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
4747
std::shared_ptr<rclcpp::Executor> executor,
4848
const std::string & manager_node_name = "controller_manager",
49-
const std::string & node_namespace = "")
49+
const std::string & node_namespace = "",
50+
const rclcpp::NodeOptions & node_options = controller_manager::get_cm_node_options())
5051
: controller_manager::ControllerManager(
51-
std::move(resource_manager), executor, manager_node_name, node_namespace)
52+
std::move(resource_manager), executor, manager_node_name, node_namespace, node_options)
5253
{
5354
}
5455
};

‎controller_manager/test/test_controllers_chaining_with_controller_manager.cpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -92,9 +92,10 @@ class TestableControllerManager : public controller_manager::ControllerManager
9292
std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
9393
std::shared_ptr<rclcpp::Executor> executor,
9494
const std::string & manager_node_name = "controller_manager",
95-
const std::string & node_namespace = "")
95+
const std::string & node_namespace = "",
96+
const rclcpp::NodeOptions & node_options = controller_manager::get_cm_node_options())
9697
: controller_manager::ControllerManager(
97-
std::move(resource_manager), executor, manager_node_name, node_namespace)
98+
std::move(resource_manager), executor, manager_node_name, node_namespace, node_options)
9899
{
99100
}
100101
};

0 commit comments

Comments
 (0)