diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index bd37fcc23c..a3f791adb6 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -203,6 +203,30 @@ class TestDiffDriveController : public ::testing::Test return controller_->init(controller_name, urdf_, 0, ns, node_options); } + bool is_configure_succeeded(const std::unique_ptr & controller) + { + auto state = controller->configure(); + return State::PRIMARY_STATE_INACTIVE == state.id(); + } + + bool is_configure_failed(const std::unique_ptr & controller) + { + auto state = controller->configure(); + return State::PRIMARY_STATE_UNCONFIGURED == state.id(); + } + + bool is_activate_succeeded(const std::unique_ptr & controller) + { + auto state = controller->get_node()->activate(); + return State::PRIMARY_STATE_ACTIVE == state.id(); + } + + bool is_activate_failed(const std::unique_ptr & controller) + { + auto state = controller->get_node()->activate(); + return State::PRIMARY_STATE_UNCONFIGURED == state.id(); + } + std::string controller_name; std::unique_ptr controller_; @@ -248,7 +272,7 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_TRUE(is_configure_failed(controller_)); } TEST_F( @@ -257,7 +281,7 @@ TEST_F( { ASSERT_EQ(InitController(), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); @@ -302,7 +326,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -327,7 +351,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -354,7 +378,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -383,7 +407,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name test_namespace), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -411,7 +435,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names test_namespace), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -440,7 +464,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name test_namespace), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -456,8 +480,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { ASSERT_EQ(InitController(), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_TRUE(is_configure_succeeded(controller_)); + + ASSERT_TRUE(is_activate_failed(controller_)); } TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) @@ -465,9 +490,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_activate_succeeded(controller_)); } TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) @@ -478,9 +505,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesVelFeedback(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_activate_succeeded(controller_)); } TEST_F(TestDiffDriveController, activate_succeeds_with_open_loop_assigned) @@ -525,12 +554,12 @@ TEST_F(TestDiffDriveController, test_speed_limiter) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + ASSERT_TRUE(is_activate_succeeded(controller_)); waitForSetup(); @@ -710,9 +739,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + ASSERT_TRUE(is_activate_failed(controller_)); } TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) @@ -723,9 +754,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesVelFeedback(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + ASSERT_TRUE(is_activate_failed(controller_)); } TEST_F(TestDiffDriveController, activate_silently_ignores_with_unnecessary_resources_assigned_1) @@ -766,12 +799,12 @@ TEST_F(TestDiffDriveController, cleanup) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + ASSERT_TRUE(is_activate_succeeded(controller_)); waitForSetup(); @@ -789,7 +822,7 @@ TEST_F(TestDiffDriveController, cleanup) EXPECT_LT(0.0, left_wheel_vel_cmd_.get_optional().value()); EXPECT_LT(0.0, right_wheel_vel_cmd_.get_optional().value()); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // should be stopped @@ -819,15 +852,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_optional().value()); EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_optional().value()); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + ASSERT_TRUE(is_activate_succeeded(controller_)); // send msg const double linear = 1.0; @@ -845,7 +877,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) // deactivated // wait so controller process the second point when deactivated std::this_thread::sleep_for(std::chrono::milliseconds(500)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -861,8 +893,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_optional().value()); EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()); - state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_TRUE(is_configure_succeeded(controller_)); + executor.cancel(); } @@ -887,12 +919,11 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) ASSERT_TRUE(controller_->set_chained_mode(false)); ASSERT_FALSE(controller_->is_in_chained_mode()); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + ASSERT_TRUE(is_activate_succeeded(controller_)); waitForSetup(); @@ -939,7 +970,7 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) std::this_thread::sleep_for(std::chrono::milliseconds(300)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -958,8 +989,8 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) << "Wheels should be halted on cleanup()"; - state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_TRUE(is_configure_succeeded(controller_)); + executor.cancel(); } @@ -983,12 +1014,11 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) ASSERT_TRUE(controller_->set_chained_mode(true)); ASSERT_TRUE(controller_->is_in_chained_mode()); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + ASSERT_TRUE(is_activate_succeeded(controller_)); waitForSetup(); @@ -1018,7 +1048,7 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) std::this_thread::sleep_for(std::chrono::milliseconds(300)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -1037,8 +1067,8 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) << "Wheels should be halted on cleanup()"; - state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_TRUE(is_configure_succeeded(controller_)); + executor.cancel(); } @@ -1047,7 +1077,7 @@ TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported) ASSERT_EQ( InitController(left_wheel_names, right_wheel_names), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_TRUE(is_configure_succeeded(controller_)); auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 2) @@ -1087,12 +1117,11 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) ASSERT_TRUE(controller_->set_chained_mode(false)); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + ASSERT_TRUE(is_activate_succeeded(controller_)); waitForSetup(); @@ -1123,7 +1152,7 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) std::this_thread::sleep_for(std::chrono::milliseconds(300)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -1135,8 +1164,7 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) << "Wheels should be halted on deactivate()"; // Activate again - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + ASSERT_TRUE(is_activate_succeeded(controller_)); waitForSetup(); @@ -1185,12 +1213,11 @@ TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_war ASSERT_TRUE(controller_->set_chained_mode(false)); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_TRUE(is_configure_succeeded(controller_)); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + ASSERT_TRUE(is_activate_succeeded(controller_)); waitForSetup(); @@ -1208,7 +1235,7 @@ TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_war // Deactivate and cleanup std::this_thread::sleep_for(std::chrono::milliseconds(300)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); state = controller_->get_node()->cleanup(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 9bf26e91d9..6e3c111cd9 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -53,6 +53,7 @@ using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; using StateType = control_msgs::msg::DynamicInterfaceGroupValues; using hardware_interface::CommandInterface; using hardware_interface::StateInterface; +using lifecycle_msgs::msg::State; namespace { @@ -120,9 +121,12 @@ class GpioCommandControllerTestSuite : public ::testing::Test void move_to_activate_state(controller_interface::return_type result_of_initialization) { ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(controller_)); + setup_command_and_state_interfaces(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_activate_succeeded(controller_)); } void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) @@ -197,6 +201,30 @@ class GpioCommandControllerTestSuite : public ::testing::Test return max_sub_check_loop_count; } + bool is_configure_succeeded(const std::unique_ptr & controller) + { + auto state = controller->configure(); + return State::PRIMARY_STATE_INACTIVE == state.id(); + } + + bool is_configure_failed(const std::unique_ptr & controller) + { + auto state = controller->configure(); + return State::PRIMARY_STATE_UNCONFIGURED == state.id(); + } + + bool is_activate_succeeded(const std::unique_ptr & controller) + { + auto state = controller->get_node()->activate(); + return State::PRIMARY_STATE_ACTIVE == state.id(); + } + + bool is_activate_failed(const std::unique_ptr & controller) + { + auto state = controller->get_node()->activate(); + return State::PRIMARY_STATE_UNCONFIGURED == state.id(); + } + std::unique_ptr controller_; const std::vector gpio_names{"gpio1", "gpio2"}; @@ -277,7 +305,8 @@ TEST_F( "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + ASSERT_TRUE(is_configure_failed(controller_)); } TEST_F( @@ -292,7 +321,8 @@ TEST_F( "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(controller_)); } TEST_F( @@ -306,7 +336,8 @@ TEST_F( const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + ASSERT_TRUE(is_configure_failed(controller_)); } TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) @@ -317,12 +348,9 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - setup_command_and_state_interfaces(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); } TEST_F( @@ -336,9 +364,9 @@ TEST_F( {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(controller_)); std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); @@ -350,7 +378,8 @@ TEST_F( state_interfaces.emplace_back(gpio_2_ana_state); controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + ASSERT_TRUE(is_activate_failed(controller_)); } TEST_F( @@ -366,7 +395,8 @@ TEST_F( const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(controller_)); std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); @@ -379,7 +409,7 @@ TEST_F( controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_TRUE(is_activate_failed(controller_)); } TEST_F( @@ -392,23 +422,9 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); - - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - std::vector command_interfaces; - command_interfaces.emplace_back(gpio_1_1_dig_cmd); - command_interfaces.emplace_back(gpio_1_2_dig_cmd); - command_interfaces.emplace_back(gpio_2_ana_cmd); - - std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_2_ana_state); - - controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); } TEST_F( @@ -646,9 +662,12 @@ TEST_F( const auto result_of_initialization = controller_->init( "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(controller_)); + controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_activate_succeeded(controller_)); auto subscription = node->create_subscription( std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, @@ -682,9 +701,12 @@ TEST_F( const auto result_of_initialization = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(controller_)); + controller_->assign_interfaces({}, std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_TRUE(is_activate_succeeded(controller_)); auto subscription = node->create_subscription( std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, diff --git a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp index e4ac047052..5cc1078362 100644 --- a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp @@ -27,13 +27,16 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/descriptions.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; using callback_return_type = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + namespace { constexpr uint16_t GPS_SERVICE = 1; @@ -60,7 +63,11 @@ class GPSSensorBroadcasterTest : public ::testing::Test public: GPSSensorBroadcasterTest() { rclcpp::init(0, nullptr); } - ~GPSSensorBroadcasterTest() { rclcpp::shutdown(); } + ~GPSSensorBroadcasterTest() + { + gps_broadcaster_.reset(nullptr); + rclcpp::shutdown(); + } void SetUp() { @@ -105,6 +112,34 @@ class GPSSensorBroadcasterTest : public ::testing::Test return gps_msg; } + bool is_configure_succeeded( + const std::unique_ptr & broadcaster) + { + auto state = broadcaster->configure(); + return State::PRIMARY_STATE_INACTIVE == state.id(); + } + + bool is_configure_failed( + const std::unique_ptr & broadcaster) + { + auto state = broadcaster->configure(); + return State::PRIMARY_STATE_UNCONFIGURED == state.id(); + } + + bool is_activate_succeeded( + const std::unique_ptr & broadcaster) + { + auto state = broadcaster->get_node()->activate(); + return State::PRIMARY_STATE_ACTIVE == state.id(); + } + + bool is_activate_failed( + const std::unique_ptr & broadcaster) + { + auto state = broadcaster->get_node()->activate(); + return State::PRIMARY_STATE_UNCONFIGURED == state.id(); + } + protected: const rclcpp::Parameter sensor_name_param_ = rclcpp::Parameter("sensor_name", "gps_sensor"); const std::string sensor_name_ = sensor_name_param_.get_value(); @@ -152,10 +187,10 @@ TEST_F( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ( - gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); - ASSERT_EQ( - gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(gps_broadcaster_)); + + ASSERT_TRUE(is_activate_succeeded(gps_broadcaster_)); } TEST_F( @@ -167,11 +202,12 @@ TEST_F( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ( - gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(gps_broadcaster_)); + setup_gps_broadcaster(); - ASSERT_EQ( - gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + ASSERT_TRUE(is_activate_succeeded(gps_broadcaster_)); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); @@ -199,11 +235,12 @@ TEST_F( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ( - gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(gps_broadcaster_)); + setup_gps_broadcaster(); - ASSERT_EQ( - gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + ASSERT_TRUE(is_activate_succeeded(gps_broadcaster_)); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); @@ -227,11 +264,12 @@ TEST_F( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ( - gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + ASSERT_TRUE(is_configure_succeeded(gps_broadcaster_)); + setup_gps_broadcaster(); - ASSERT_EQ( - gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + ASSERT_TRUE(is_activate_succeeded(gps_broadcaster_)); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value());