Skip to content

Commit

Permalink
Add list_controllers service
Browse files Browse the repository at this point in the history
  • Loading branch information
Victor Lopez committed Sep 18, 2020
1 parent bef08e2 commit 27206ea
Show file tree
Hide file tree
Showing 3 changed files with 157 additions and 12 deletions.
7 changes: 6 additions & 1 deletion controller_manager/include/controller_manager/controller_manager.hpp
View file
Expand Up @@ -93,7 +93,7 @@ class ControllerManager : public rclcpp::Node

CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type
configure() const;
configure();

CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type
Expand Down Expand Up @@ -149,6 +149,11 @@ class ControllerManager : public rclcpp::Node
/// The index of the controllers list being used in the real-time thread.
int used_by_realtime_ = {-1};

/// mutex copied from ROS1 Control, protects service callbacks
/// not needed if we're guaranteed that the callbacks don't come from multiple threads
std::mutex services_lock_;
rclcpp::Service<controller_manager_msgs::srv::ListControllers>::SharedPtr
list_controllers_service_;

std::vector<controller_interface::ControllerInterface *> start_request_, stop_request_;
#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING
Expand Down
47 changes: 46 additions & 1 deletion controller_manager/src/controller_manager.cpp
View file
Expand Up @@ -580,6 +580,45 @@ void ControllerManager::start_controllers_asap()
#endif
}

void ControllerManager::list_controllers_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>,
std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response)
{
// lock services
RCLCPP_DEBUG(get_logger(), "list controller service called");
std::lock_guard<std::mutex> services_guard(services_lock_);
RCLCPP_DEBUG(get_logger(), "list controller service locked");

// lock controllers to get all names/types/states
std::lock_guard<std::recursive_mutex> controller_guard(controllers_lock_);
auto & controllers = controllers_lists_[current_controllers_list_];
response->controller.resize(controllers.size());

for (size_t i = 0; i < controllers.size(); ++i) {
controller_manager_msgs::msg::ControllerState & cs = response->controller[i];
cs.name = controllers[i].info.name;
cs.type = controllers[i].info.type;
cs.state = controllers[i].c->get_lifecycle_node()->get_current_state().label();

#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING
cs.claimed_resources.clear();
typedef std::vector<hardware_interface::InterfaceResources> ClaimedResVec;
typedef ClaimedResVec::const_iterator ClaimedResIt;
const ClaimedResVec & c_resources = controllers[i].info.claimed_resources;
for (const auto & c_resource : c_resources) {
controller_manager_msgs::HardwareInterfaceResources iface_res;
iface_res.hardware_interface = c_resource.hardware_interface;
std::copy(
c_resource.resources.begin(), c_resource.resources.end(),
std::back_inserter(iface_res.resources));
cs.claimed_resources.push_back(iface_res);
}
#endif
}

RCLCPP_DEBUG(get_logger(), "list controller service finished");
}

controller_interface::return_type
ControllerManager::update()
{
Expand All @@ -604,8 +643,14 @@ ControllerManager::update()
}

controller_interface::return_type
ControllerManager::configure() const
ControllerManager::configure()
{
using namespace std::placeholders;
list_controllers_service_ = create_service<controller_manager_msgs::srv::ListControllers>(
"list_controllers", std::bind(
&ControllerManager::list_controllers_srv_cb, this, _1,
_2));

auto ret = controller_interface::return_type::SUCCESS;
for (auto loaded_controller : controllers_lists_[current_controllers_list_]) {
auto controller_state = loaded_controller.c->get_lifecycle_node()->configure();
Expand Down
115 changes: 105 additions & 10 deletions controller_manager/test/test_controller_manager.cpp
View file
Expand Up @@ -17,7 +17,8 @@
#include <string>

#include "controller_manager/controller_manager.hpp"

#include "controller_manager_msgs/srv/list_controllers.hpp"
#include "controller_manager_msgs/srv/switch_controller.hpp"
#include "lifecycle_msgs/msg/state.hpp"

#include "rclcpp/utilities.hpp"
Expand Down Expand Up @@ -47,40 +48,134 @@ class TestControllerManager : public ::testing::Test
};

TEST_F(TestControllerManager, controller_lifecycle) {
controller_manager::ControllerManager cm(robot, executor, "test_controller_manager");
auto cm = std::make_shared<controller_manager::ControllerManager>(
robot, executor,
"test_controller_manager");


auto test_controller = std::make_shared<test_controller::TestController>();
auto abstract_test_controller = cm.add_controller(
auto abstract_test_controller = cm->add_controller(
test_controller, "test_controller",
"TestControllerType");
EXPECT_EQ(1u, cm.get_loaded_controllers().size());
EXPECT_EQ(1u, cm->get_loaded_controllers().size());

EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.update());
EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update());
EXPECT_EQ(
0u,
test_controller->internal_counter) <<
"Update should not reached an unconfigured and deactivated controller";

EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.configure());
EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->configure());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_node()->get_current_state().id());

EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.activate());
EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->activate());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
test_controller->get_lifecycle_node()->get_current_state().id());

EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.update());
EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update());
EXPECT_EQ(1u, test_controller->internal_counter);

EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.deactivate());
EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->deactivate());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_node()->get_current_state().id());

EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.cleanup());
EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->cleanup());
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_node()->get_current_state().id());
}

std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> call_service_and_wait(
rclcpp::Client<controller_manager_msgs::srv::ListControllers> & client,
std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,
rclcpp::Executor & service_executor)
{
EXPECT_TRUE(client.wait_for_service(std::chrono::milliseconds(500)));
auto result = client.async_send_request(request);
// Wait for the result.
EXPECT_EQ(
service_executor.spin_until_future_complete(result),
rclcpp::FutureReturnCode::SUCCESS);
return result.get();
}

TEST_F(TestControllerManager, list_controllers_srv) {
auto cm = std::make_shared<controller_manager::ControllerManager>(
robot, executor,
"test_controller_manager");

// periodic calls to update
auto timer = cm->create_wall_timer(
std::chrono::milliseconds(10),
std::bind(&controller_manager::ControllerManager::update, cm.get()));

executor->add_node(cm);
EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->configure());

auto future_handle = std::async(
std::launch::async, [this]() -> void {
executor->spin();
});

rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
srv_executor.add_node(srv_node);
rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr client =
srv_node->create_client<controller_manager_msgs::srv::ListControllers>("list_controllers");
auto request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>();

auto result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(
0u,
result->controller.size());


auto test_controller = std::make_shared<test_controller::TestController>();
static const std::string CONTROLLER_NAME = "test_controller";
static const std::string CONTROLLER_TYPE = "TestControllerType";
auto abstract_test_controller = cm->add_controller(
test_controller, "test_controller",
"TestControllerType");
EXPECT_EQ(1u, cm->get_loaded_controllers().size());
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(
1u,
result->controller.size());
ASSERT_EQ(CONTROLLER_NAME, result->controller[0].name);
ASSERT_EQ(CONTROLLER_TYPE, result->controller[0].type);
ASSERT_EQ("inactive", result->controller[0].state);

cm->switch_controller(
{CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));

result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(
1u,
result->controller.size());
ASSERT_EQ("active", result->controller[0].state);


cm->switch_controller(
{}, {CONTROLLER_NAME},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));

result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(
1u,
result->controller.size());
ASSERT_EQ("inactive", result->controller[0].state);

cm->unload_controller(CONTROLLER_NAME);
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(
0u,
result->controller.size());
executor->cancel();
}

0 comments on commit 27206ea

Please sign in to comment.