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.

Sign up for GitHub

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

Fix controller starting with later load of robot description test #1624

Merged
merged 3 commits into from
Jul 19, 2024
Merged

Fix controller starting with later load of robot description test #1624

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4b5ce9b
Fix the duplicated restart of the controller_manager services initial…
saikishor Jul 19, 2024
b976964
Scope the ControllerManagerRunner to avoid malloc and other test issues
saikishor Jul 19, 2024
e92305b
reorder the tests for consistent log at the end
saikishor Jul 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 15 additions & 24 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,20 +220,7 @@ ControllerManager::ControllerManager(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
{
if (!get_parameter("update_rate", update_rate_))
{
RCLCPP_WARN(
get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
}

if (is_resource_manager_initialized())
{
init_services();
}

diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
init_controller_manager();
Copy link
Member

@bmagyar bmagyar Jul 19, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did we have all this duplicated code here the whole time?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Well only from the #1358. So it is not so long

}

ControllerManager::ControllerManager(
Expand Down Expand Up @@ -262,17 +249,20 @@ void ControllerManager::init_controller_manager()
get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
}

robot_description_notification_timer_ = create_wall_timer(
std::chrono::seconds(1),
[&]()
{
RCLCPP_WARN(
get_logger(), "Waiting for data on 'robot_description' topic to finish initialization");
});
if (is_resource_manager_initialized())
{
init_services();
}
else
{
robot_description_notification_timer_ = create_wall_timer(
std::chrono::seconds(1),
[&]()
{
RCLCPP_WARN(
get_logger(), "Waiting for data on 'robot_description' topic to finish initialization");
});
}

// set QoS to transient local to get messages that have already been published
// (if robot state publisher starts before controller manager)
Expand Down Expand Up @@ -305,6 +295,10 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
init_resource_manager(robot_description_);
if (is_resource_manager_initialized())
{
RCLCPP_INFO(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't this a debug print?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes! it is. I think it is very helpful to know somehow via log that it initialized properly and we are going to start the services. If you want, I can remove it as well. I'm fine with anything

get_logger(),
"Resource Manager has been successfully initialized. Starting Controller Manager "
"services...");
init_services();
}
}
Expand Down Expand Up @@ -374,9 +368,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
resource_manager_->set_component_state(component, active_state);
}

// Init CM services first after the URDF is loaded an components are set
init_services();
robot_description_notification_timer_->cancel();
}

Expand Down
116 changes: 59 additions & 57 deletions controller_manager/test/test_spawner_unspawner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -327,6 +327,61 @@ TEST_F(TestLoadController, unload_on_kill)
ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
}

TEST_F(TestLoadController, spawner_test_fallback_controllers)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why was this moved?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I moved it for better log tracking as the newly added test is placed in between an original test.

ros2_control/controller_manager/test/test_spawner_unspawner.cpp

Lines 330 to 407 in 9d1aff9

class TestLoadControllerWithoutRobotDescription
: public ControllerManagerFixture<controller_manager::ControllerManager>
{
public:
TestLoadControllerWithoutRobotDescription()
: ControllerManagerFixture<controller_manager::ControllerManager>("")
{
}
void SetUp() override
{
ControllerManagerFixture::SetUp();
update_timer_ = cm_->create_wall_timer(
std::chrono::milliseconds(10),
[&]()
{
cm_->read(time_, PERIOD);
cm_->update(time_, PERIOD);
cm_->write(time_, PERIOD);
});
update_executor_ =
std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(), 2);
update_executor_->add_node(cm_);
update_executor_spin_future_ =
std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
// This sleep is needed to prevent a too fast test from ending before the
// executor has began to spin, which causes it to hang
std::this_thread::sleep_for(50ms);
}
void TearDown() override { update_executor_->cancel(); }
rclcpp::TimerBase::SharedPtr robot_description_sending_timer_;
protected:
rclcpp::TimerBase::SharedPtr update_timer_;
// Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
std::shared_ptr<rclcpp::Executor> update_executor_;
std::future<void> update_executor_spin_future_;
};
TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out)
{
cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 256)
<< "could not spawn controller because not robot description and not services for controller "
"manager are active";
}
TEST_F(
TestLoadControllerWithoutRobotDescription,
controller_starting_with_later_load_of_robot_description)
{
cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
// Delay sending robot description
robot_description_sending_timer_ = cm_->create_wall_timer(
std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); });
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0)
<< "could not activate control because not robot description";
ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
}
}

{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
"/test/test_controller_spawner_with_fallback_controllers.yaml";

cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));

ControllerManagerRunner cm_runner(this);
EXPECT_EQ(
call_spawner(
"ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 "
"-p " +
test_file_path),
0);

ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}

// Try to spawn now the controller with fallback controllers inside the yaml
EXPECT_EQ(
call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0);

ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_3 = cm_->get_loaded_controllers()[2];
ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}
}

class TestLoadControllerWithoutRobotDescription
: public ControllerManagerFixture<controller_manager::ControllerManager>
{
Expand Down Expand Up @@ -393,70 +448,17 @@ TEST_F(
robot_description_sending_timer_ = cm_->create_wall_timer(
std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); });

ControllerManagerRunner cm_runner(this);
EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0)
<< "could not activate control because not robot description";

ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0)
<< "could not activate control because not robot description";
}
}

TEST_F(TestLoadController, spawner_test_fallback_controllers)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
"/test/test_controller_spawner_with_fallback_controllers.yaml";

cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));

ControllerManagerRunner cm_runner(this);
EXPECT_EQ(
call_spawner(
"ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 "
"-p " +
test_file_path),
0);

ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}

// Try to spawn now the controller with fallback controllers inside the yaml
EXPECT_EQ(
call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0);

ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_3 = cm_->get_loaded_controllers()[2];
ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
}
}
Loading