1#include "modulo_components/LifecycleComponent.hpp"
4using namespace rclcpp_lifecycle;
9 : LifecycleNode(modulo_utils::parsing::parse_node_name(node_options, fallback_name), node_options),
10 ComponentInterface(std::make_shared<rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>(
11 LifecycleNode::get_node_base_interface(), LifecycleNode::get_node_clock_interface(),
12 LifecycleNode::get_node_graph_interface(), LifecycleNode::get_node_logging_interface(),
13 LifecycleNode::get_node_parameters_interface(), LifecycleNode::get_node_services_interface(),
14 LifecycleNode::get_node_time_source_interface(), LifecycleNode::get_node_timers_interface(),
15 LifecycleNode::get_node_topics_interface(), LifecycleNode::get_node_type_descriptions_interface(),
16 LifecycleNode::get_node_waitables_interface())) {}
18std::shared_ptr<state_representation::ParameterInterface>
23void LifecycleComponent::step() {
26 this->evaluate_periodic_callbacks();
28 this->publish_outputs();
30 this->publish_predicates();
31 }
catch (
const std::exception&
ex) {
38node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_configure(
const State& previous_state) {
39 RCLCPP_DEBUG(this->
get_logger(),
"on_configure called from previous state %s", previous_state.label().c_str());
40 if (
previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) {
42 return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
44 if (!this->handle_configure()) {
47 if (this->handle_cleanup()) {
48 return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
52 "Could not revert to the unconfigured state! Entering into the error processing transition state.");
53 return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
56 return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
59bool LifecycleComponent::handle_configure() {
63 }
catch (
const std::exception&
ex) {
67 return result && this->configure_outputs();
74node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_cleanup(
const State& previous_state) {
75 RCLCPP_DEBUG(this->
get_logger(),
"on_cleanup called from previous state %s", previous_state.label().c_str());
76 if (
previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
78 return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
80 if (!this->handle_cleanup()) {
82 return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
84 return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
87bool LifecycleComponent::handle_cleanup() {
90 }
catch (
const std::exception&
ex) {
100node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_activate(
const State& previous_state) {
101 RCLCPP_DEBUG(this->
get_logger(),
"on_activate called from previous state %s", previous_state.label().c_str());
102 if (
previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
104 return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
106 if (!this->handle_activate()) {
109 if (this->handle_deactivate()) {
110 return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
113 get_logger(),
"Could not revert to the inactive state! Entering into the error processing transition state.");
114 return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
117 return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
120bool LifecycleComponent::handle_activate() {
124 }
catch (
const std::exception&
ex) {
128 return result && this->activate_outputs();
135node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_deactivate(
const State& previous_state) {
136 RCLCPP_DEBUG(this->
get_logger(),
"on_deactivate called from previous state %s", previous_state.label().c_str());
137 if (
previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
139 return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
141 if (!this->handle_deactivate()) {
143 return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
145 return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
148bool LifecycleComponent::handle_deactivate() {
149 auto result = this->deactivate_outputs();
152 }
catch (
const std::exception&
ex) {
162node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_shutdown(
const State& previous_state) {
163 RCLCPP_DEBUG(this->
get_logger(),
"on_shutdown called from previous state %s", previous_state.label().c_str());
165 case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED:
166 return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
167 case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE:
168 if (!this->handle_deactivate()) {
173 case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE:
174 if (!this->handle_cleanup()) {
179 case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED:
180 if (!this->handle_shutdown()) {
188 return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
194 return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
197bool LifecycleComponent::handle_shutdown() {
201 }
catch (
const std::exception&
ex) {
205 return result && this->clear_signals();
212node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_error(
const State& previous_state) {
213 RCLCPP_DEBUG(this->
get_logger(),
"on_error called from previous state %s", previous_state.label().c_str());
217 error_handled = this->handle_error();
218 }
catch (
const std::exception&
ex) {
225 return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
228 return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
231bool LifecycleComponent::handle_error() {
239bool LifecycleComponent::configure_outputs() {
248 case MessageType::BOOL: {
251 PublisherType::LIFECYCLE_PUBLISHER, publisher)
252 ->create_publisher_interface(message_pair);
255 case MessageType::FLOAT64: {
256 auto publisher = this->create_publisher<std_msgs::msg::Float64>(topic_name, this->get_qos());
258 std::make_shared<
PublisherHandler<LifecyclePublisher<std_msgs::msg::Float64>, std_msgs::msg::Float64>>(
259 PublisherType::LIFECYCLE_PUBLISHER, publisher)
260 ->create_publisher_interface(message_pair);
263 case MessageType::FLOAT64_MULTI_ARRAY: {
264 auto publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(topic_name, this->get_qos());
266 LifecyclePublisher<std_msgs::msg::Float64MultiArray>, std_msgs::msg::Float64MultiArray>>(
267 PublisherType::LIFECYCLE_PUBLISHER, publisher)
268 ->create_publisher_interface(message_pair);
271 case MessageType::INT32: {
272 auto publisher = this->create_publisher<std_msgs::msg::Int32>(topic_name, this->get_qos());
274 std::make_shared<
PublisherHandler<LifecyclePublisher<std_msgs::msg::Int32>, std_msgs::msg::Int32>>(
275 PublisherType::LIFECYCLE_PUBLISHER, publisher)
276 ->create_publisher_interface(message_pair);
279 case MessageType::STRING: {
280 auto publisher = this->create_publisher<std_msgs::msg::String>(topic_name, this->get_qos());
282 std::make_shared<
PublisherHandler<LifecyclePublisher<std_msgs::msg::String>, std_msgs::msg::String>>(
283 PublisherType::LIFECYCLE_PUBLISHER, publisher)
284 ->create_publisher_interface(message_pair);
287 case MessageType::ENCODED_STATE: {
288 auto publisher = this->create_publisher<modulo_core::EncodedState>(topic_name, this->get_qos());
289 interface = std::make_shared<
291 PublisherType::LIFECYCLE_PUBLISHER, publisher)
292 ->create_publisher_interface(message_pair);
295 case MessageType::CUSTOM_MESSAGE: {
296 interface = this->custom_output_configuration_callables_.at(name)(topic_name);
302 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to configure output '" << name <<
"': " << ex.what());
308bool LifecycleComponent::clear_signals() {
309 RCLCPP_DEBUG(this->get_logger(),
"Clearing all inputs and outputs.");
310 this->inputs_.clear();
311 this->outputs_.clear();
315bool LifecycleComponent::activate_outputs() {
317 for (
auto const& [name, interface] : this->outputs_) {
319 interface->activate();
322 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to activate output '" << name <<
"': " << ex.what());
325 RCLCPP_DEBUG(this->get_logger(),
"All outputs activated.");
329bool LifecycleComponent::deactivate_outputs() {
331 for (
auto const& [name, interface] : this->outputs_) {
333 interface->deactivate();
336 RCLCPP_ERROR_STREAM(this->get_logger(),
"Failed to deactivate output '" << name <<
"': " << ex.what());
339 RCLCPP_DEBUG(this->get_logger(),
"All outputs deactivated.");
343rclcpp_lifecycle::State LifecycleComponent::get_lifecycle_state()
const {
344 return this->get_current_state();
Base interface class for modulo components to wrap a ROS Node with custom behaviour.
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
virtual void on_step_callback()
Steps to execute periodically. To be redefined by derived Component classes.
void set_predicate(const std::string &predicate_name, bool predicate_value)
Set the value of the predicate given as parameter, if the predicate is not found does not do anything...
rclcpp::QoS get_qos() const
Getter of the Quality of Service attribute.
virtual bool on_shutdown_callback()
Steps to execute when shutting down the component.
virtual bool on_deactivate_callback()
Steps to execute when deactivating the component.
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
virtual bool on_activate_callback()
Steps to execute when activating the component.
virtual bool on_cleanup_callback()
Steps to execute when cleaning up the component.
LifecycleComponent(const rclcpp::NodeOptions &node_options, const std::string &fallback_name="LifecycleComponent")
Constructor from node options.
virtual bool on_error_callback()
Steps to execute when handling errors.
virtual bool on_configure_callback()
Steps to execute when configuring the component.
rclcpp_lifecycle::State get_lifecycle_state() const
Get the current lifecycle state of the component.
The PublisherHandler handles different types of ROS publishers to activate, deactivate and publish da...
A base class for all core exceptions.
Modulo Core communication module for handling messages on publication and subscription interfaces.