diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
index d07cced4b1f5d1ef885718c748727a803dc8a1ed..4daedb2eaf8145616630ba49eaefeef5962fce46 100644
--- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
+++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
@@ -63,8 +63,11 @@ protected:
void shutdownAllNodes();
void destroyLifecycleServiceClients();
+ // For a node, transition to the new target state
+ bool changeStateForNode(const std::string & node_name, std::uint8_t transition);
+
// For each node in the map, transition to the new target state
- void changeStateForAllNodes(std::uint8_t transition);
+ bool changeStateForAllNodes(std::uint8_t transition);
// Convenience function to highlight the output on the console
void message(const std::string & msg);
@@ -72,6 +75,9 @@ protected:
// A map of all nodes to be controlled
std::map<std::string, std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;
+ // A map of the expected transitions to primary states
+ std::unordered_map<std::uint8_t, std::uint8_t> transition_state_map_;
+
// The names of the nodes to be managed, in the order of desired bring-up
std::vector<std::string> node_names_;
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
index 0b07c7f660896443a0542965b9cb2a5466675094..ff98b2a8c03c416c0e60b872b806c0ac3ea6369a 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp
+++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -25,6 +25,7 @@ using namespace std::chrono_literals;
using namespace std::placeholders;
using lifecycle_msgs::msg::Transition;
+using lifecycle_msgs::msg::State;
using nav2_util::LifecycleServiceClient;
namespace nav2_lifecycle_manager
@@ -56,6 +57,14 @@ LifecycleManager::LifecycleManager()
auto options = rclcpp::NodeOptions().arguments(
{std::string("__node:=") + get_name() + "service_client"});
service_client_node_ = std::make_shared<rclcpp::Node>("_", options);
+
+ transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
+ transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED;
+ transition_state_map_[Transition::TRANSITION_ACTIVATE] = State::PRIMARY_STATE_ACTIVE;
+ transition_state_map_[Transition::TRANSITION_DEACTIVATE] = State::PRIMARY_STATE_INACTIVE;
+ transition_state_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
+ State::PRIMARY_STATE_FINALIZED;
+
if (autostart_) {
startup();
}
@@ -103,29 +112,38 @@ LifecycleManager::destroyLifecycleServiceClients()
}
}
-void
+bool
+LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t transition)
+{
+ if (!node_map_[node_name]->change_state(transition) ||
+ !(node_map_[node_name]->get_state() == transition_state_map_[transition]))
+ {
+ RCLCPP_ERROR(get_logger(), "Failed to change state for node: %s", node_name.c_str());
+ return false;
+ }
+
+ return true;
+}
+
+bool
LifecycleManager::changeStateForAllNodes(std::uint8_t transition)
{
for (const auto & kv : node_map_) {
- if (!kv.second->change_state(transition)) {
- RCLCPP_ERROR(get_logger(), "Failed to change state for node: %s", kv.first.c_str());
- return;
+ if (!changeStateForNode(kv.first, transition)) {
+ return false;
}
}
+
+ return true;
}
bool
LifecycleManager::bringupNode(const std::string & node_name)
{
message(std::string("Configuring and activating ") + node_name);
- if (!node_map_[node_name]->change_state(Transition::TRANSITION_CONFIGURE)) {
- RCLCPP_ERROR(get_logger(), "Failed to configure node: %s", node_name.c_str());
- return false;
- }
-
- auto rc = node_map_[node_name]->change_state(Transition::TRANSITION_ACTIVATE);
- if (!rc) {
- RCLCPP_ERROR(get_logger(), "Failed to activate node: %s", node_name.c_str());
+ if (!changeStateForNode(node_name, Transition::TRANSITION_CONFIGURE) ||
+ !changeStateForNode(node_name, Transition::TRANSITION_ACTIVATE))
+ {
return false;
}
@@ -148,7 +166,8 @@ LifecycleManager::startup()
createLifecycleServiceClients();
for (auto & node_name : node_names_) {
if (!bringupNode(node_name)) {
- RCLCPP_ERROR(get_logger(), "Failed to bring up node: %s, aboring bringup", node_name.c_str());
+ RCLCPP_ERROR(get_logger(), "Failed to bring up node: %s, aborting bringup",
+ node_name.c_str());
return;
}
}