From 0ea1f49dc7e2364570c8dfac713c7e375d2db91a Mon Sep 17 00:00:00 2001 From: Maciej Bednarczyk Date: Wed, 3 Aug 2022 13:00:43 +0200 Subject: [PATCH] fixed tests and added gpio state tests --- .../test/test_gpio_command_controller.cpp | 96 ++++++++++++++++++- .../test/test_gpio_command_controller.hpp | 6 ++ 2 files changed, 98 insertions(+), 4 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index c6a7340a32..6e97c404bd 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -27,9 +27,11 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_gpio_command_controller.hpp" -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; using CmdType = std_msgs::msg::Float64MultiArray; +using StateType = control_msgs::msg::DynamicJointState; namespace { @@ -62,7 +64,13 @@ void GpioCommandControllerTest::SetUpController() command_ifs.emplace_back(gpio_1_1_dig_cmd_); command_ifs.emplace_back(gpio_1_2_dig_cmd_); command_ifs.emplace_back(gpio_2_ana_cmd_); - controller_->assign_interfaces(std::move(command_ifs), {}); + + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state_); + state_ifs.emplace_back(gpio_1_2_dig_state_); + state_ifs.emplace_back(gpio_2_ana_state_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } TEST_F(GpioCommandControllerTest, GpiosParameterNotSet) @@ -132,6 +140,7 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet ASSERT_EQ( @@ -170,6 +179,8 @@ TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + // send command with wrong number of gpios auto command_ptr = std::make_shared(); @@ -198,6 +209,7 @@ TEST_F(GpioCommandControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet ASSERT_EQ( @@ -218,6 +230,10 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); controller_->get_node()->set_parameter( {"command_interfaces.gpio2", std::vector{"ana.1"}}); + controller_->get_node()->set_parameter( + {"state_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"state_interfaces.gpio2", std::vector{"ana.1"}}); // default values @@ -225,6 +241,10 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_state_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_state_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_state_.get_value(), 3.1); + auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -233,9 +253,9 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( + auto command_pub = test_node.create_publisher( std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - std_msgs::msg::Float64MultiArray command_msg; + CmdType command_msg; command_msg.data = {0.0, 1.0, 30.0}; command_pub->publish(command_msg); @@ -255,3 +275,71 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); } + +TEST_F(GpioCommandControllerTest, StateCallbackTest) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", gpio_names_}); + controller_->get_node()->set_parameter( + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"command_interfaces.gpio2", std::vector{"ana.1"}}); + controller_->get_node()->set_parameter( + {"state_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"state_interfaces.gpio2", std::vector{"ana.1"}}); + + + // default values + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + + ASSERT_EQ(gpio_1_1_dig_state_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_state_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_state_.get_value(), 3.1); + + auto node_state = controller_->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // send a new command + rclcpp::Node test_node("test_node"); + auto subs_callback = [&](const StateType::SharedPtr) {}; + auto subscription = test_node.create_subscription( + std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, subs_callback); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.joint_names.size(), gpio_names_.size()); + ASSERT_EQ(gpio_state_msg.joint_names[0], "gpio1"); + ASSERT_EQ(gpio_state_msg.joint_names[1], "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values[0].interface_names[0], "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values[0].interface_names[1], "dig.2"); + ASSERT_EQ(gpio_state_msg.interface_values[1].interface_names[0], "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values[0].values[0], 1.0); + ASSERT_EQ(gpio_state_msg.interface_values[0].values[1], 0.0); + ASSERT_EQ(gpio_state_msg.interface_values[1].values[0], 3.1); +} diff --git a/gpio_controllers/test/test_gpio_command_controller.hpp b/gpio_controllers/test/test_gpio_command_controller.hpp index 6627dee5b9..9841ed4add 100644 --- a/gpio_controllers/test/test_gpio_command_controller.hpp +++ b/gpio_controllers/test/test_gpio_command_controller.hpp @@ -26,6 +26,7 @@ #include "gpio_controllers/gpio_command_controller.hpp" using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; // subclassing and friending so we can access member variables class FriendGpioCommandController : public gpio_controllers::GpioCommandController @@ -53,10 +54,15 @@ class GpioCommandControllerTest : public ::testing::Test // dummy gpio state values used for tests const std::vector gpio_names_ = {"gpio1", "gpio2"}; std::vector gpio_commands_ = {1.0, 0.0, 3.1}; + std::vector gpio_states_ = {1.0, 0.0, 3.1}; CommandInterface gpio_1_1_dig_cmd_{gpio_names_[0], "dig.1", &gpio_commands_[0]}; CommandInterface gpio_1_2_dig_cmd_{gpio_names_[0], "dig.2", &gpio_commands_[1]}; CommandInterface gpio_2_ana_cmd_{gpio_names_[1], "ana.1", &gpio_commands_[2]}; + + StateInterface gpio_1_1_dig_state_{gpio_names_[0], "dig.1", &gpio_states_[0]}; + StateInterface gpio_1_2_dig_state_{gpio_names_[0], "dig.2", &gpio_states_[1]}; + StateInterface gpio_2_ana_state_{gpio_names_[1], "ana.1", &gpio_states_[2]}; }; #endif // TEST_GPIO_COMMAND_CONTROLLER_HPP_