Skip to content

Remove service nodes in lifecycle_manager #2267

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

Merged
SteveMacenski merged 8 commits into from
Mar 29, 2021
Merged

Remove service nodes in lifecycle_manager #2267

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
3f6ded4
remove service_node from nav2_lifecycle_manager
Mar 10, 2021
921ec0c
wait_for_service() returns the value of service_is_ready()
Mar 10, 2021
cbed614
Merge remote-tracking branch 'origin/main' into remove_service_nodes
Mar 24, 2021
a37a6ac
explain wait_for_service return
Mar 24, 2021
597df82
use timer to trigger init()
Mar 25, 2021
366c2cb
init() as lambda function
Mar 26, 2021
1f9706e
wait_for_service as void
Mar 26, 2021
ddf48e1
fix gtest
Mar 26, 2021
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
9 changes: 7 additions & 2 deletions nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,13 @@ class LifecycleManager : public rclcpp::Node
*/
~LifecycleManager();

/**
* @brief init create service cleant and start if autostart is true

@SteveMacenski SteveMacenski Mar 25, 2021

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

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

Suggested change
* @brief init create service cleant and start if autostart is true
* @brief init create service clients and start if autostart is true

*/
void init();

protected:
// The ROS node to use when calling lifecycle services
rclcpp::Node::SharedPtr service_client_node_;
// The ROS node to create bond
rclcpp::Node::SharedPtr bond_client_node_;
std::unique_ptr<nav2_util::NodeThread> bond_node_thread_;

Expand Down Expand Up @@ -169,6 +173,7 @@ class LifecycleManager : public rclcpp::Node
void message(const std::string & msg);

// Timer thread to look at bond connections
rclcpp::TimerBase::SharedPtr init_timer_;
rclcpp::TimerBase::SharedPtr bond_timer_;
std::chrono::milliseconds bond_timeout_;

Expand Down
24 changes: 15 additions & 9 deletions nav2_lifecycle_manager/src/lifecycle_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,8 @@ LifecycleManager::LifecycleManager()
get_name() + std::string("/is_active"),
std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3));

auto service_options = rclcpp::NodeOptions().arguments(
{"--ros-args", "-r", std::string("__node:=") + get_name() + "_service_client", "--"});
auto bond_options = rclcpp::NodeOptions().arguments(
{"--ros-args", "-r", std::string("__node:=") + get_name() + "_bond_client", "--"});
service_client_node_ = std::make_shared<rclcpp::Node>("_", service_options);
bond_client_node_ = std::make_shared<rclcpp::Node>("_", bond_options);
bond_node_thread_ = std::make_unique<nav2_util::NodeThread>(bond_client_node_);

Expand All @@ -79,18 +76,27 @@ LifecycleManager::LifecycleManager()
transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
std::string("Shutting down ");

createLifecycleServiceClients();

if (autostart_) {
startup();
}
init_timer_ = this->create_wall_timer(
std::chrono::milliseconds(100),
std::bind(&LifecycleManager::init, this));
}

LifecycleManager::~LifecycleManager()
{
RCLCPP_INFO(get_logger(), "Destroying %s", get_name());
Comment thread
SteveMacenski marked this conversation as resolved.
}

void
LifecycleManager::init()
Comment thread
SteveMacenski marked this conversation as resolved.
Outdated
{
init_timer_->cancel();
Comment thread
SteveMacenski marked this conversation as resolved.
Outdated
createLifecycleServiceClients();

if (autostart_) {
startup();
}
}

void
LifecycleManager::managerCallback(
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
Expand Down Expand Up @@ -131,7 +137,7 @@ LifecycleManager::createLifecycleServiceClients()
message("Creating and initializing lifecycle service clients");
for (auto & node_name : node_names_) {
node_map_[node_name] =
std::make_shared<LifecycleServiceClient>(node_name, service_client_node_);
std::make_shared<LifecycleServiceClient>(node_name, shared_from_this());
}
}

Expand Down
4 changes: 3 additions & 1 deletion nav2_util/include/nav2_util/service_client.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,8 +125,9 @@ class ServiceClient
/**
* @brief Block until a service is available
* @param timeout Maximum timeout to wait for, default infinite
* @return true if the client is ready
*/
void wait_for_service(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max())
bool wait_for_service(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max())

@SteveMacenski SteveMacenski Mar 24, 2021

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

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

Why a bool? It throws an exception if its not ready, there's no need to return the bool if success/failed because you'll know if an exception wasn't thrown

@BriceRenaudeau BriceRenaudeau Mar 25, 2021

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

The exception is thrown only if rclcpp is not ok.
And it might be better to remove the while loop and only keep wait_for_service(timeout)?
I don't really get the behavior with it.
https://github.com/ros-planning/navigation2/blob/ed43bed86253961f2723bb38389e51cabf6e7ebf/nav2_util/include/nav2_util/service_client.hpp#L132-L138
And using a return to get the result seems cleaner than using a try/catch.

@BriceRenaudeau BriceRenaudeau Mar 25, 2021

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

return client_->wait_for_service(timeout);

Should be sufficient?

@SteveMacenski SteveMacenski Mar 25, 2021

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

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

This is an infinite while loop, this will never return a non-successful state because it will block forever or throw an exception in a failure state.

{
auto sleep_dur = std::chrono::milliseconds(10);
while (!client_->wait_for_service(timeout)) {
Expand All @@ -136,6 +137,7 @@ class ServiceClient
}
rclcpp::sleep_for(sleep_dur);
}
return !client_->service_is_ready();
}

protected:
Expand Down