Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add reference timeout parameter to controllers templates #111

Open
wants to merge 17 commits into
base: master
Choose a base branch
from
Prev Previous commit
Next Next commit
regular controller added missing tests
  • Loading branch information
GiridharBukka committed Mar 14, 2023
commit e2cc41ed1b9d967ed8c4d867c3999b74bc9831dd
154 changes: 152 additions & 2 deletions templates/ros2_control/controller/test_dummy_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ class DummyClassNameTest : public DummyClassNameFixture<TestableDummyClassName>
{
};


TEST_F(DummyClassNameTest, when_controller_is_configured_expect_all_parameters_set)
{
SetUpController();
Expand All @@ -45,7 +44,7 @@ TEST_F(DummyClassNameTest, when_controller_is_configured_expect_all_parameters_s
ASSERT_EQ(controller_->params_.interface_name, interface_name_);
}

TEST_F(DummyClassNameTest, check_exported_intefaces)
TEST_F(DummyClassNameTest, when_controller_configured_expect_properly_exported_interfaces)
{
SetUpController();

Expand Down Expand Up @@ -258,6 +257,157 @@ TEST_F(DummyClassNameTest, receive_message_and_publish_updated_status)
ASSERT_EQ(msg.set_point, 0.45);
}

// when too old msg is sent expect reference msg reset
TEST_F(DummyClassNameTest, when_reference_msg_is_too_old_expect_unset_reference)
{
SetUpController();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(controller_->get_node()->get_node_base_interface());

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

auto reference = *(controller_->input_ref_.readFromNonRT());
auto old_timestamp = reference->header.stamp;
EXPECT_TRUE(std::isnan(reference->displacements[0]));

// reference_callback() is implicitly called when publish_commands() is called
// reference_msg is published with provided time stamp when publish_commands( time_stamp)
// is called
publish_commands(
controller_->get_node()->now() - controller_->ref_timeout_ -
rclcpp::Duration::from_seconds(0.1));
ASSERT_TRUE(controller_->wait_for_commands(executor));
ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp);
EXPECT_TRUE(std::isnan(reference->displacements[0]));
}

// when not in chainable mode and ref_msg_timedout expect
// command_interfaces are set to 0.0 and when ref_msg is not timedout expect
// command_interfaces are set to valid command values
TEST_F(DummyClassNameTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds)
{
// 1. age>ref_timeout 2. age<ref_timeout
SetUpController();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(controller_->get_node()->get_node_base_interface());

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
controller_->set_chained_mode(false);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_FALSE(controller_->is_in_chained_mode());

joint_command_values_[0] = TEST_DISPLACEMENT;
std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
rclcpp::Duration::from_seconds(0.1);
msg->joint_names = joint_names_;
msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
msg->duration = std::numeric_limits<double>::quiet_NaN();
controller_->input_ref_.writeFromNonRT(msg);
const auto age_of_last_command =
controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;

// age_of_last_command > ref_timeout_
ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
ASSERT_EQ(
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);

EXPECT_EQ(joint_command_values_[0], 0.0);

for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) {
EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0);
}

std::shared_ptr<ControllerReferenceMsg> msg_2 = std::make_shared<ControllerReferenceMsg>();
msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01);
msg_2->joint_names = joint_names_;
msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
msg_2->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
msg_2->duration = std::numeric_limits<double>::quiet_NaN();
controller_->input_ref_.writeFromNonRT(msg_2);
const auto age_of_last_command_2 =
controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;
EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST);
// age_of_last_command_2 < ref_timeout_
ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_);
ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
ASSERT_EQ(
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);
EXPECT_EQ(joint_command_values_[0], TEST_DISPLACEMENT);
ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);

for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) {
EXPECT_EQ(controller_->command_interfaces_[i].get_value(), TEST_DISPLACEMENT);
}
}

// when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces
// are calculated to valid values and reference_interfaces are unset
TEST_F(DummyClassNameTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once)
{
SetUpController();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(controller_->get_node()->get_node_base_interface());
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

joint_command_values_[0] = TEST_DISPLACEMENT;
controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0);
msg->joint_names = joint_names_;
msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
msg->duration = std::numeric_limits<double>::quiet_NaN();
controller_->input_ref_.writeFromNonRT(msg);
const auto age_of_last_command =
controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;

ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
ASSERT_EQ(
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
controller_interface::return_type::OK);
EXPECT_FALSE(std::isnan(joint_command_values_[0]));
ASSERT_NE((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
EXPECT_EQ(joint_command_values_[NR_STATE_ITFS], TEST_DISPLACEMENT);
ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0]));
}

TEST_F(
DummyClassNameTest,
when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once)
{
SetUpController();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(controller_->get_node()->get_node_base_interface());
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size());
EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]);
EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
// reference_callback() is called implicitly when publish_commands() is called.
publish_commands(controller_->get_node()->now());
ASSERT_TRUE(controller_->wait_for_commands(executor));
EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size());
EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]);
EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45);
EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0);
EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25);
}

int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);
Expand Down