Skip to content

Commit 9f13b60

Browse files
Vatan Aksoy Tezerdestoglbmagyar
authored
Use lifecycle nodes in controllers again (#538)
* Add lifecycle nodes * Add custom 'configure' to controller interface to get 'update_rate' parameter. * Disable external interfaces of LifecycleNode. Co-authored-by: Denis Štogl <denis@stogl.de> Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
1 parent 6b495ef commit 9f13b60

File tree

8 files changed

+85
-183
lines changed

8 files changed

+85
-183
lines changed

‎controller_interface/include/controller_interface/controller_interface.hpp‎

Lines changed: 19 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -23,10 +23,9 @@
2323

2424
#include "hardware_interface/loaned_command_interface.hpp"
2525
#include "hardware_interface/loaned_state_interface.hpp"
26-
#include "lifecycle_msgs/msg/state.hpp"
2726

2827
#include "rclcpp/rclcpp.hpp"
29-
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
28+
#include "rclcpp_lifecycle/lifecycle_node.hpp"
3029

3130
namespace controller_interface
3231
{
@@ -80,16 +79,30 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
8079
void release_interfaces();
8180

8281
CONTROLLER_INTERFACE_PUBLIC
83-
virtual LifecycleNodeInterface::CallbackReturn on_init() = 0;
82+
virtual return_type init(const std::string & controller_name);
8483

84+
/// Custom configure method to read additional parameters for controller-nodes
85+
/*
86+
* Override default implementation for configure of LifecycleNode to get parameters.
87+
*/
8588
CONTROLLER_INTERFACE_PUBLIC
86-
virtual return_type init(const std::string & controller_name);
89+
const rclcpp_lifecycle::State & configure();
90+
91+
/// Extending interface with initialization method which is individual for each controller
92+
CONTROLLER_INTERFACE_PUBLIC
93+
virtual LifecycleNodeInterface::CallbackReturn on_init() = 0;
8794

8895
CONTROLLER_INTERFACE_PUBLIC
8996
virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
9097

9198
CONTROLLER_INTERFACE_PUBLIC
92-
std::shared_ptr<rclcpp::Node> get_node();
99+
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();
100+
101+
CONTROLLER_INTERFACE_PUBLIC
102+
const rclcpp_lifecycle::State & get_state() const;
103+
104+
CONTROLLER_INTERFACE_PUBLIC
105+
unsigned int get_update_rate() const;
93106

94107
/// Declare and initialize a parameter with a type.
95108
/**
@@ -111,42 +124,10 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
111124
}
112125
}
113126

114-
/**
115-
* The methods below are a substitute to the LifecycleNode methods with the same name.
116-
* The Life cycle is shown in ROS2 design document:
117-
* https://design.ros2.org/articles/node_lifecycle.html
118-
* We cannot use a LifecycleNode because it would expose change-state services to the rest
119-
* of the ROS system.
120-
* Only the Controller Manager should have possibility to change state of a controller.
121-
*
122-
* Hopefully in the future we can use a LifecycleNode where we disable modifications from the outside.
123-
*/
124-
CONTROLLER_INTERFACE_PUBLIC
125-
const rclcpp_lifecycle::State & configure();
126-
127-
CONTROLLER_INTERFACE_PUBLIC
128-
const rclcpp_lifecycle::State & cleanup();
129-
130-
CONTROLLER_INTERFACE_PUBLIC
131-
const rclcpp_lifecycle::State & deactivate();
132-
133-
CONTROLLER_INTERFACE_PUBLIC
134-
const rclcpp_lifecycle::State & activate();
135-
136-
CONTROLLER_INTERFACE_PUBLIC
137-
const rclcpp_lifecycle::State & shutdown();
138-
139-
CONTROLLER_INTERFACE_PUBLIC
140-
const rclcpp_lifecycle::State & get_state() const;
141-
142-
CONTROLLER_INTERFACE_PUBLIC
143-
unsigned int get_update_rate() const;
144-
145127
protected:
146128
std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
147129
std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
148-
std::shared_ptr<rclcpp::Node> node_;
149-
rclcpp_lifecycle::State lifecycle_state_;
130+
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
150131
unsigned int update_rate_ = 0;
151132
};
152133

‎controller_interface/src/controller_interface.cpp‎

Lines changed: 43 additions & 123 deletions
Original file line numberDiff line numberDiff line change
@@ -20,148 +20,63 @@
2020
#include <vector>
2121

2222
#include "hardware_interface/types/lifecycle_state_names.hpp"
23-
#include "lifecycle_msgs/msg/state.hpp"
2423

2524
namespace controller_interface
2625
{
2726
return_type ControllerInterface::init(const std::string & controller_name)
2827
{
29-
node_ = std::make_shared<rclcpp::Node>(
30-
controller_name, rclcpp::NodeOptions()
31-
.allow_undeclared_parameters(true)
32-
.automatically_declare_parameters_from_overrides(true));
28+
node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
29+
controller_name,
30+
rclcpp::NodeOptions()
31+
.allow_undeclared_parameters(true)
32+
.automatically_declare_parameters_from_overrides(true),
33+
false); // disable LifecycleNode service interfaces
3334

34-
return_type result = return_type::OK;
35-
switch (on_init())
35+
try
3636
{
37-
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
38-
lifecycle_state_ = rclcpp_lifecycle::State(
39-
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
40-
hardware_interface::lifecycle_state_names::UNCONFIGURED);
41-
break;
42-
case LifecycleNodeInterface::CallbackReturn::ERROR:
43-
case LifecycleNodeInterface::CallbackReturn::FAILURE:
44-
result = return_type::ERROR;
45-
break;
37+
auto_declare<int>("update_rate", 0);
4638
}
47-
return result;
48-
}
49-
50-
const rclcpp_lifecycle::State & ControllerInterface::configure()
51-
{
52-
if (lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
39+
catch (const std::exception & e)
5340
{
54-
switch (on_configure(lifecycle_state_))
55-
{
56-
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
57-
lifecycle_state_ = rclcpp_lifecycle::State(
58-
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
59-
hardware_interface::lifecycle_state_names::INACTIVE);
60-
break;
61-
case LifecycleNodeInterface::CallbackReturn::ERROR:
62-
on_error(lifecycle_state_);
63-
lifecycle_state_ = rclcpp_lifecycle::State(
64-
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
65-
hardware_interface::lifecycle_state_names::FINALIZED);
66-
break;
67-
case LifecycleNodeInterface::CallbackReturn::FAILURE:
68-
break;
69-
}
70-
71-
if (node_->has_parameter("update_rate"))
72-
{
73-
update_rate_ = node_->get_parameter("update_rate").as_int();
74-
}
41+
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
42+
return return_type::ERROR;
7543
}
76-
return lifecycle_state_;
77-
}
7844

79-
const rclcpp_lifecycle::State & ControllerInterface::cleanup()
80-
{
81-
switch (on_cleanup(lifecycle_state_))
82-
{
83-
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
84-
lifecycle_state_ = rclcpp_lifecycle::State(
85-
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
86-
hardware_interface::lifecycle_state_names::UNCONFIGURED);
87-
break;
88-
case LifecycleNodeInterface::CallbackReturn::ERROR:
89-
on_error(lifecycle_state_);
90-
lifecycle_state_ = rclcpp_lifecycle::State(
91-
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
92-
hardware_interface::lifecycle_state_names::FINALIZED);
93-
break;
94-
case LifecycleNodeInterface::CallbackReturn::FAILURE:
95-
break;
96-
}
97-
return lifecycle_state_;
98-
}
99-
const rclcpp_lifecycle::State & ControllerInterface::deactivate()
100-
{
101-
switch (on_deactivate(lifecycle_state_))
45+
switch (on_init())
10246
{
10347
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
104-
lifecycle_state_ = rclcpp_lifecycle::State(
105-
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
106-
hardware_interface::lifecycle_state_names::INACTIVE);
10748
break;
10849
case LifecycleNodeInterface::CallbackReturn::ERROR:
109-
on_error(lifecycle_state_);
110-
lifecycle_state_ = rclcpp_lifecycle::State(
111-
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
112-
hardware_interface::lifecycle_state_names::FINALIZED);
113-
break;
11450
case LifecycleNodeInterface::CallbackReturn::FAILURE:
115-
break;
51+
return return_type::ERROR;
11652
}
117-
return lifecycle_state_;
118-
}
119-
const rclcpp_lifecycle::State & ControllerInterface::activate()
120-
{
121-
if (lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
122-
{
123-
switch (on_activate(lifecycle_state_))
124-
{
125-
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
126-
lifecycle_state_ = rclcpp_lifecycle::State(
127-
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
128-
hardware_interface::lifecycle_state_names::ACTIVE);
129-
break;
130-
case LifecycleNodeInterface::CallbackReturn::ERROR:
131-
on_error(lifecycle_state_);
132-
lifecycle_state_ = rclcpp_lifecycle::State(
133-
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
134-
hardware_interface::lifecycle_state_names::FINALIZED);
135-
break;
136-
case LifecycleNodeInterface::CallbackReturn::FAILURE:
137-
break;
138-
}
139-
}
140-
return lifecycle_state_;
53+
54+
node_->register_on_configure(
55+
std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1));
56+
57+
node_->register_on_cleanup(
58+
std::bind(&ControllerInterface::on_cleanup, this, std::placeholders::_1));
59+
60+
node_->register_on_activate(
61+
std::bind(&ControllerInterface::on_activate, this, std::placeholders::_1));
62+
63+
node_->register_on_deactivate(
64+
std::bind(&ControllerInterface::on_deactivate, this, std::placeholders::_1));
65+
66+
node_->register_on_shutdown(
67+
std::bind(&ControllerInterface::on_shutdown, this, std::placeholders::_1));
68+
69+
node_->register_on_error(std::bind(&ControllerInterface::on_error, this, std::placeholders::_1));
70+
71+
return return_type::OK;
14172
}
14273

143-
const rclcpp_lifecycle::State & ControllerInterface::shutdown()
74+
const rclcpp_lifecycle::State & ControllerInterface::configure()
14475
{
145-
switch (on_shutdown(lifecycle_state_))
146-
{
147-
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
148-
lifecycle_state_ = rclcpp_lifecycle::State(
149-
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
150-
hardware_interface::lifecycle_state_names::FINALIZED);
151-
break;
152-
case LifecycleNodeInterface::CallbackReturn::ERROR:
153-
on_error(lifecycle_state_);
154-
lifecycle_state_ = rclcpp_lifecycle::State(
155-
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
156-
hardware_interface::lifecycle_state_names::FINALIZED);
157-
break;
158-
case LifecycleNodeInterface::CallbackReturn::FAILURE:
159-
break;
160-
}
161-
return lifecycle_state_;
162-
}
76+
update_rate_ = node_->get_parameter("update_rate").as_int();
16377

164-
const rclcpp_lifecycle::State & ControllerInterface::get_state() const { return lifecycle_state_; }
78+
return node_->configure();
79+
}
16580

16681
void ControllerInterface::assign_interfaces(
16782
std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
@@ -177,11 +92,16 @@ void ControllerInterface::release_interfaces()
17792
state_interfaces_.clear();
17893
}
17994

180-
std::shared_ptr<rclcpp::Node> ControllerInterface::get_node()
95+
const rclcpp_lifecycle::State & ControllerInterface::get_state() const
96+
{
97+
return node_->get_current_state();
98+
}
99+
100+
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterface::get_node()
181101
{
182102
if (!node_.get())
183103
{
184-
throw std::runtime_error("Node hasn't been initialized yet!");
104+
throw std::runtime_error("Lifecycle node hasn't been initialized yet!");
185105
}
186106
return node_;
187107
}

‎controller_interface/test/test_controller_with_options.hpp‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -44,14 +44,11 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
4444
{
4545
rclcpp::NodeOptions options;
4646
options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
47-
node_ = std::make_shared<rclcpp::Node>(controller_name, options);
47+
node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(controller_name, options);
4848

4949
switch (on_init())
5050
{
5151
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
52-
lifecycle_state_ = rclcpp_lifecycle::State(
53-
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
54-
hardware_interface::lifecycle_state_names::UNCONFIGURED);
5552
break;
5653
case LifecycleNodeInterface::CallbackReturn::ERROR:
5754
case LifecycleNodeInterface::CallbackReturn::FAILURE:

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ inline bool is_controller_inactive(
5656
return is_controller_inactive(*controller);
5757
}
5858

59-
inline bool is_controller_active(controller_interface::ControllerInterface & controller)
59+
inline bool is_controller_active(const controller_interface::ControllerInterface & controller)
6060
{
6161
return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
6262
}
@@ -312,8 +312,8 @@ controller_interface::return_type ControllerManager::unload_controller(
312312
}
313313

314314
RCLCPP_DEBUG(get_logger(), "Cleanup controller");
315-
controller.c->cleanup();
316-
executor_->remove_node(controller.c->get_node());
315+
controller.c->get_node()->cleanup();
316+
executor_->remove_node(controller.c->get_node()->get_node_base_interface());
317317
to.erase(found_it);
318318

319319
// Destroys the old controllers list when the realtime thread is finished with it.
@@ -371,7 +371,7 @@ controller_interface::return_type ControllerManager::configure_controller(
371371
{
372372
RCLCPP_DEBUG(
373373
get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str());
374-
new_state = controller->cleanup();
374+
new_state = controller->get_node()->cleanup();
375375
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
376376
{
377377
RCLCPP_ERROR(
@@ -724,7 +724,7 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_contro
724724
controller.info.name.c_str());
725725
controller.c->get_node()->set_parameter(use_sim_time);
726726
}
727-
executor_->add_node(controller.c->get_node());
727+
executor_->add_node(controller.c->get_node()->get_node_base_interface());
728728
to.emplace_back(controller);
729729

730730
// Destroys the old controllers list when the realtime thread is finished with it.
@@ -782,7 +782,7 @@ void ControllerManager::stop_controllers()
782782
auto controller = found_it->c;
783783
if (is_controller_active(*controller))
784784
{
785-
const auto new_state = controller->deactivate();
785+
const auto new_state = controller->get_node()->deactivate();
786786
controller->release_interfaces();
787787
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
788788
{
@@ -893,7 +893,7 @@ void ControllerManager::start_controllers()
893893
}
894894
controller->assign_interfaces(std::move(command_loans), std::move(state_loans));
895895

896-
const auto new_state = controller->activate();
896+
const auto new_state = controller->get_node()->activate();
897897
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
898898
{
899899
RCLCPP_ERROR(
@@ -1355,7 +1355,7 @@ controller_interface::return_type ControllerManager::update(
13551355
// https://github.com/ros-controls/ros2_control/issues/153
13561356
if (is_controller_active(*loaded_controller.c))
13571357
{
1358-
auto controller_update_rate = loaded_controller.c->get_update_rate();
1358+
const auto controller_update_rate = loaded_controller.c->get_update_rate();
13591359

13601360
bool controller_go =
13611361
controller_update_rate == 0 || ((update_loop_counter_ % controller_update_rate) == 0);

‎controller_manager/test/test_controller/test_controller.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@ TestController::TestController()
3030
controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
3131
{
3232
if (
33-
lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
34-
lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
33+
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
34+
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
3535
{
3636
return cmd_iface_cfg_;
3737
}
@@ -45,8 +45,8 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c
4545
controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
4646
{
4747
if (
48-
lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
49-
lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
48+
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
49+
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
5050
{
5151
return state_iface_cfg_;
5252
}

0 commit comments

Comments
 (0)