Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix registration logic in lifecycle manager #52

Merged
merged 6 commits into from
Jan 8, 2025
Merged

Fix registration logic in lifecycle manager #52

Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
9e4d2f4
Fix registration logic in lifecycle manager
luca-della-vedova Jan 7, 2025
da8e425
Make sure to use state, not transition
luca-della-vedova Jan 7, 2025
410b119
Remove _system_active in favor of lifecycle state
luca-della-vedova Jan 7, 2025
c3200d5
Convert target state to optional
luca-della-vedova Jan 7, 2025
967fade
Add warning
luca-della-vedova Jan 8, 2025
40e1473
Update nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecy…
luca-della-vedova Jan 8, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
117 changes: 37 additions & 80 deletions nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,10 +87,9 @@ class Implementation
std::shared_ptr<nexus::common::LifecycleServiceClient<rclcpp::Node>>>
_node_clients;

bool _system_active{false};
std::chrono::seconds _service_request_timeout{10s};
// autostart, if set to true, will transition the first added node to ACTIVE
bool _autostart{false};
// Lifecycle state that managed nodes should be in
std::optional<uint8_t> _target_state = std::nullopt;

std::shared_ptr<std::thread> spin_thread_;
// Callback group used by services
Expand All @@ -117,11 +116,17 @@ class Implementation
{
std::optional<uint8_t> state = client->get_state();
if (!state.has_value())
{
ok = false;
RCLCPP_WARN(
this->node_->get_logger(), "Failed to get state for node [%s]",
luca-della-vedova marked this conversation as resolved.
Show resolved Hide resolved
name.c_str());
continue;
Yadunund marked this conversation as resolved.
Show resolved Hide resolved
}

ok = ok && (
transitionGraph.atGoalState(state.value(), transition) ||
client->change_state(transition));
ok &= transitionGraph.atGoalState(state.value(), transition) ||
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: By switching to bitwise AND we lose the optimization of short circuiting if ok is false so we can we keep the original implementation? Let me know if I missed something else.

Also not sure whats the point of returning a boolean in this function since we might have successfully transitioned some nodes and failed for others and the user would not know which ones succeeded. But that is beyond the scope of this PR.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is an interesting one, the idea behind this change is that previously as soon as ok became false the short circuiting would make all the following clients skip transitioning as well.
With the new logic without short circuiting instead, all the following client will still transition.
This is also why I added the continue above, before the short circuiting would make sure we never call .value() even as new nodes do indeed have a state but since short circuiting is removed we need an early guard clause

client->change_state(transition);
this->_target_state = state.value();
}
return ok;
}
Expand All @@ -131,7 +136,7 @@ class Implementation
const std::shared_ptr<std_srvs::srv::Trigger::Request> /*request*/,
std::shared_ptr<std_srvs::srv::Trigger::Response> response)
{
response->success = this->_system_active;
response->success = this->_target_state == State::PRIMARY_STATE_ACTIVE;
}

void shutdownAllNodes()
Expand Down Expand Up @@ -164,14 +169,11 @@ class Implementation
return false;
}
this->message("Managed nodes are active");
this->_system_active = true;
return true;
}

bool shutdown()
{
this->_system_active = false;

this->message("Shutting down managed nodes...");
this->shutdownAllNodes();
this->destroyLifecycleServiceClients();
Expand All @@ -181,8 +183,6 @@ class Implementation

bool reset()
{
this->_system_active = false;

this->message("Resetting managed nodes...");
// Should transition in reverse order
if (!this->changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE) ||
Expand All @@ -199,8 +199,6 @@ class Implementation
}
bool pause()
{
this->_system_active = false;

this->message("Pausing managed nodes...");
if (!this->changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE))
{
Expand All @@ -226,7 +224,6 @@ class Implementation
}

this->message("Managed nodes are active");
this->_system_active = true;
return true;
}

Expand Down Expand Up @@ -336,78 +333,34 @@ class Implementation

bool add_node(const std::string& name)
{
// If this is the first node, then the node will be transitioned to active
if (this->_node_clients.size() == 0)
uint8_t currentState;
if (!this->GetState(name, currentState))
{
uint8_t currentState;
if (!this->GetState(name, currentState))
{
message("The state of current nodes were not recheable, "
"Keep the new node is unconfigure state");
return false;
}

if (_autostart)
{
std::vector<int> solution = transitionGraph.dijkstra(currentState, 3);
for (unsigned int i = 0; i < solution.size(); i++)
{
if (!ChangeState(name, solution[i]))
{
RCLCPP_ERROR(
this->node_->get_logger(),
"Not able to transition the node [%s]. This node is not inserted",
name.c_str());
return false;
}
currentState = solution[i];
}
}
_node_clients.insert_or_assign(name,
std::make_shared<nexus::common::LifecycleServiceClient<rclcpp::Node>>(
name));
return true;
message("The state of new node to add was not reacheable. "
"This node is not inserted");
return false;
}
else

// If the target state is different from the current, transition this node to it
if (this->_target_state.has_value())
{
if (_node_clients.size() > 0)
std::vector<int> solution = transitionGraph.dijkstra(currentState, this->_target_state.value());
for (unsigned int i = 0; i < solution.size(); i++)
{
uint8_t targetState;
if (!this->GetState(this->_node_clients.begin()->first, targetState))
{
message("The state of current nodes were not recheable, "
"Keep the new node is unconfigure state");
return false;
}

uint8_t currentState;
if (!this->GetState(name, currentState))
if (!ChangeState(name, solution[i]))
{
message("The state of new node to add was not recheable, "
"Keep the new node is unconfigure state");
RCLCPP_ERROR(
this->node_->get_logger(),
"Not able to transition the node [%s]. This node is not inserted",
name.c_str());
return false;
}

std::vector<int> solution = transitionGraph.dijkstra(currentState,
targetState);
for (unsigned int i = 0; i < solution.size(); i++)
{
if (!ChangeState(name, solution[i]))
{
RCLCPP_ERROR(
this->node_->get_logger(),
"Not able to transition the node [%s]. This node is not inserted",
name.c_str());
return false;
}
}
}
_node_clients.insert_or_assign(name,
std::make_shared<nexus::common::LifecycleServiceClient<rclcpp::Node>>(
name));
return true;
}
return false;
_node_clients.insert_or_assign(name,
std::make_shared<nexus::common::LifecycleServiceClient<rclcpp::Node>>(
name));
return true;
}

bool change_state(const std::string& node_name, const std::uint8_t goal)
Expand Down Expand Up @@ -465,7 +418,11 @@ class LifecycleManager
{
_pimpl = rmf_utils::make_impl<Implementation<NodeType>>();
_pimpl->_are_services_active = activate_services;
_pimpl->_autostart = autostart;
// Set the target state to active if autostart is enabled
if (autostart)
{
_pimpl->_target_state = State::PRIMARY_STATE_ACTIVE;
}
_pimpl->_service_request_timeout = std::chrono::seconds(
service_request_timeout);

Expand Down Expand Up @@ -552,7 +509,7 @@ class LifecycleManager
return _pimpl->add_node(node_name_);
}

/// \brief REmove a node from the list
/// \brief Remove a node from the list
/// \param[in] node_name_ Node name to remove
/// \return True if the node name was remove from the list, false otherwise
bool removeNodeName(const std::string& node_name_)
Expand Down
Loading