Skip to content

Commit

Permalink
Fix controller starting with later load of robot description test (#1624
Browse files Browse the repository at this point in the history 
)

* Fix the duplicated restart of the controller_manager services initialization

* Scope the ControllerManagerRunner to avoid malloc and other test issues

* reorder the tests for consistent log at the end
  • Loading branch information
@saikishor
saikishor authored Jul 19, 2024
1 parent 152d19b commit 0792a35
Show file tree
Hide file tree
Showing 2 changed files with 74 additions and 81 deletions.
39 changes: 15 additions & 24 deletions controller_manager/src/controller_manager.cpp
View file
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();
}

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(
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
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)
{
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);
}
}

0 comments on commit 0792a35

Please sign in to comment.