Skip to content

Commit

Permalink
Added functionality and tests for getting robot description from the …
Browse files Browse the repository at this point in the history 
…topic.
  • Loading branch information
@destogl
destogl committed Dec 18, 2023
1 parent ac71462 commit 21c43a1
Show file tree
Hide file tree
Showing 8 changed files with 287 additions and 77 deletions.
1 change: 1 addition & 0 deletions controller_manager/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,6 +150,7 @@ if(BUILD_TESTING)
)
target_link_libraries(test_controller_manager_urdf_passing
controller_manager
test_controller
ros2_control_test_assets::ros2_control_test_assets
)
ament_target_dependencies(test_controller_manager_urdf_passing
Expand Down
3 changes: 2 additions & 1 deletion controller_manager/include/controller_manager/controller_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -322,7 +322,7 @@ class ControllerManager : public rclcpp::Node
std::vector<std::string> get_controller_names();
std::pair<std::string, std::string> split_command_interface(
const std::string & command_interface);
void subscribe_to_robot_description_topic();
void init_controller_manager();

/**
* Clear request lists used when switching controllers. The lists are shared between "callback"
Expand Down Expand Up @@ -536,6 +536,7 @@ class ControllerManager : public rclcpp::Node

std::string robot_description_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;

struct SwitchParams
{
Expand Down
65 changes: 41 additions & 24 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -267,18 +267,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.");
}

subscribe_to_robot_description_topic();

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

init_services();
init_controller_manager();
}

ControllerManager::ControllerManager(
Expand All @@ -295,21 +284,17 @@ ControllerManager::ControllerManager(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
{
init_controller_manager();
}

void ControllerManager::init_controller_manager()
{
// Get parameters needed for RT "update" loop to work
if (!get_parameter("update_rate", update_rate_))
{
RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
}

subscribe_to_robot_description_topic();

diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
init_services();
}

void ControllerManager::subscribe_to_robot_description_topic()
{
// set QoS to transient local to get messages that have already been published
// (if robot state publisher starts before controller manager)
robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
Expand All @@ -318,6 +303,19 @@ void ControllerManager::subscribe_to_robot_description_topic()
RCLCPP_INFO(
get_logger(), "Subscribing to '%s' topic for robot description.",
robot_description_subscription_->get_topic_name());

// Setup diagnostics
diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);

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");
});
}

void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
Expand All @@ -339,8 +337,14 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &

void ControllerManager::init_resource_manager(const std::string & robot_description)
{
// TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong...
resource_manager_->load_urdf(robot_description);
if (!resource_manager_->load_urdf(robot_description))
{
RCLCPP_WARN(
get_logger(),
"URDF validation went wrong check the previous output. This might only mean that interfaces "
"defined in URDF and exported by the hardware do not match. Therefore continue initializing "
"controller manager...");
}

// Get all components and if they are not defined in parameters activate them automatically
auto components_to_activate = resource_manager_->get_components_status();
Expand Down Expand Up @@ -396,6 +400,10 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
{
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();
}

void ControllerManager::init_services()
Expand Down Expand Up @@ -752,6 +760,15 @@ controller_interface::return_type ControllerManager::switch_controller(
const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
const rclcpp::Duration & timeout)
{
if (!resource_manager_->is_urdf_already_loaded())
{
RCLCPP_ERROR(
get_logger(),
"Resource Manager is not initialized yet! Please provide robot description on "
"'~/robot_description' topic before trying to switch controllers.");
return controller_interface::return_type::ERROR;
}

switch_params_ = SwitchParams();

if (!deactivate_request_.empty() || !activate_request_.empty())
Expand Down
156 changes: 132 additions & 24 deletions controller_manager/test/test_controller_manager_urdf_passing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,14 @@ class TestableControllerManager : public controller_manager::ControllerManager
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed);
FRIEND_TEST(
TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error);
TestControllerManagerWithTestableCM,
when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description);
FRIEND_TEST(
TestControllerManagerWithTestableCM,
when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description);
FRIEND_TEST(
TestControllerManagerWithTestableCM,
when_starting_controller_expect_error_before_rm_is_initialized_after_some_time);

public:
TestableControllerManager(
Expand All @@ -58,6 +65,45 @@ class TestControllerManagerWithTestableCM
public:
// create cm with no urdf
TestControllerManagerWithTestableCM() : ControllerManagerFixture<TestableControllerManager>("") {}

void prepare_controller()
{
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
test_controller_ = std::make_shared<test_controller::TestController>();
cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME);
ASSERT_NE(test_controller_, nullptr);
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id());
}

void configure_and_try_switch_that_returns_error()
{
// configure controller
cm_->configure_controller(CONTROLLER_NAME);
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id());

// Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
cm_->get_logger().set_level(rclcpp::Logger::Level::Debug);

switch_test_controllers(
strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::ready,
controller_interface::return_type::ERROR);

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id());
}

void try_successful_switch()
{
switch_test_controllers(
strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout,
controller_interface::return_type::OK);

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id());
}

std::shared_ptr<test_controller::TestController> test_controller_;
};

TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called)
Expand All @@ -80,30 +126,92 @@ TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
}

TEST_P(TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error)
TEST_P(
TestControllerManagerWithTestableCM,
when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description)
{
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
auto controller_if = cm_->load_controller(CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME);
ASSERT_NE(controller_if, nullptr);
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id());
cm_->configure_controller(CONTROLLER_NAME);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id());

// Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
cm_->get_logger().set_level(rclcpp::Logger::Level::Debug);

switch_test_controllers(
strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout,
controller_interface::return_type::ERROR);

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id());

// mimic callback
// auto msg = std_msgs::msg::String();
// msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf;
// cm_->robot_description_callback(msg);
// ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
prepare_controller();

// setup state interfaces to claim from controllers
controller_interface::InterfaceConfiguration state_itfs_cfg;
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
{
state_itfs_cfg.names.push_back(interface);
}
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
{
state_itfs_cfg.names.push_back(interface);
}
test_controller_->set_state_interface_configuration(state_itfs_cfg);

configure_and_try_switch_that_returns_error();

pass_robot_description_to_cm_and_rm();
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());

try_successful_switch();
}

TEST_P(
TestControllerManagerWithTestableCM,
when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description)
{
prepare_controller();

// setup command interfaces to claim from controllers
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
{
cmd_itfs_cfg.names.push_back(interface);
}
test_controller_->set_command_interface_configuration(cmd_itfs_cfg);

configure_and_try_switch_that_returns_error();

pass_robot_description_to_cm_and_rm();
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());

try_successful_switch();
}

TEST_P(
TestControllerManagerWithTestableCM,
when_starting_controller_expect_error_before_rm_is_initialized_after_some_time)
{
prepare_controller();

// setup state interfaces to claim from controllers
controller_interface::InterfaceConfiguration state_itfs_cfg;
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
{
state_itfs_cfg.names.push_back(interface);
}
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
{
state_itfs_cfg.names.push_back(interface);
}
test_controller_->set_state_interface_configuration(state_itfs_cfg);

// setup command interfaces to claim from controllers
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
{
cmd_itfs_cfg.names.push_back(interface);
}
test_controller_->set_command_interface_configuration(cmd_itfs_cfg);

configure_and_try_switch_that_returns_error();

std::this_thread::sleep_for(std::chrono::milliseconds(3000));

pass_robot_description_to_cm_and_rm();
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());

try_successful_switch();
}

INSTANTIATE_TEST_SUITE_P(
Expand Down
Loading

0 comments on commit 21c43a1

Please sign in to comment.