Skip to content

Commit

Permalink
fixed tests and added gpio state tests
Browse files Browse the repository at this point in the history
  • Loading branch information
Maciej Bednarczyk committed Aug 3, 2022
1 parent 577ebcc commit 0ea1f49
Show file tree
Hide file tree
Showing 2 changed files with 98 additions and 4 deletions.
96 changes: 92 additions & 4 deletions gpio_controllers/test/test_gpio_command_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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<LoanedStateInterface> 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)
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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<CmdType>();
Expand Down Expand Up @@ -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(
Expand All @@ -218,13 +230,21 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest)
{"command_interfaces.gpio1", std::vector<std::string>{"dig.1", "dig.2"}});
controller_->get_node()->set_parameter(
{"command_interfaces.gpio2", std::vector<std::string>{"ana.1"}});
controller_->get_node()->set_parameter(
{"state_interfaces.gpio1", std::vector<std::string>{"dig.1", "dig.2"}});
controller_->get_node()->set_parameter(
{"state_interfaces.gpio2", std::vector<std::string>{"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);

Expand All @@ -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<std_msgs::msg::Float64MultiArray>(
auto command_pub = test_node.create_publisher<CmdType>(
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);

Expand All @@ -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<std::string>{"dig.1", "dig.2"}});
controller_->get_node()->set_parameter(
{"command_interfaces.gpio2", std::vector<std::string>{"ana.1"}});
controller_->get_node()->set_parameter(
{"state_interfaces.gpio1", std::vector<std::string>{"dig.1", "dig.2"}});
controller_->get_node()->set_parameter(
{"state_interfaces.gpio2", std::vector<std::string>{"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<StateType>(
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);
}
6 changes: 6 additions & 0 deletions gpio_controllers/test/test_gpio_command_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -53,10 +54,15 @@ class GpioCommandControllerTest : public ::testing::Test
// dummy gpio state values used for tests
const std::vector<std::string> gpio_names_ = {"gpio1", "gpio2"};
std::vector<double> gpio_commands_ = {1.0, 0.0, 3.1};
std::vector<double> 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_

0 comments on commit 0ea1f49

Please sign in to comment.