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 3142ae5e8c32a0ea153b73def3e1eb051beab7cd..b1347c8080ba0199eb14068e44012f6bd31cfe60 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -24,10 +24,13 @@ #include "nav2_util/lifecycle_service_client.hpp" #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/empty.hpp" +#include "nav2_msgs/srv/manage_nodes.hpp" namespace nav2_lifecycle_manager { +using nav2_msgs::srv::ManageNodes; + class LifecycleManager : public rclcpp::Node { public: @@ -39,22 +42,19 @@ protected: rclcpp::Node::SharedPtr service_client_node_; // The services provided by this node - rclcpp::Service<std_srvs::srv::Empty>::SharedPtr startup_srv_; - rclcpp::Service<std_srvs::srv::Empty>::SharedPtr shutdown_srv_; - - void startupCallback( - const std::shared_ptr<rmw_request_id_t> request_header, - const std::shared_ptr<std_srvs::srv::Empty::Request> request, - std::shared_ptr<std_srvs::srv::Empty::Response> response); + rclcpp::Service<ManageNodes>::SharedPtr manager_srv_; - void shutdownCallback( + void managerCallback( const std::shared_ptr<rmw_request_id_t> request_header, - const std::shared_ptr<std_srvs::srv::Empty::Request> request, - std::shared_ptr<std_srvs::srv::Empty::Response> response); + const std::shared_ptr<ManageNodes::Request> request, + std::shared_ptr<ManageNodes::Response> response); // Support functions for the service calls - void startup(); - void shutdown(); + bool startup(); + bool shutdown(); + bool reset(); + bool pause(); + bool resume(); // Support functions for bring-up void createLifecycleServiceClients(); diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index c161d5056c9f283ad4b49472541014791fcb1372..088128733e53db41abdc8fe24bb4294f207fe1e5 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -23,6 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "std_srvs/srv/empty.hpp" +#include "nav2_msgs/srv/manage_nodes.hpp" namespace nav2_lifecycle_manager { @@ -41,20 +42,15 @@ public: bool navigate_to_pose(double x, double y, double theta); protected: - using Empty = std_srvs::srv::Empty; + using ManageNodes = nav2_msgs::srv::ManageNodes; // A generic method used to call startup, shutdown, etc. - void callService(rclcpp::Client<Empty>::SharedPtr service_client, const char * service_name); + void callService(uint8_t command); // The node to use for the service call rclcpp::Node::SharedPtr node_; - // The same (empty) request for all of the services - std::shared_ptr<Empty::Request> request_; - - // The service clients - rclcpp::Client<Empty>::SharedPtr startup_client_; - rclcpp::Client<Empty>::SharedPtr shutdown_client_; + rclcpp::Client<ManageNodes>::SharedPtr manager_client_; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 1f0ccaee4b27799b0dd2d131332ebc80f38f9e0d..5f1a76738f6c005b9c1bc7ee02d77a2f84d3a6cb 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -26,6 +26,7 @@ using namespace std::placeholders; using lifecycle_msgs::msg::Transition; using lifecycle_msgs::msg::State; +using nav2_msgs::msg::Command; using nav2_util::LifecycleServiceClient; namespace nav2_lifecycle_manager @@ -48,11 +49,8 @@ LifecycleManager::LifecycleManager() get_parameter("node_names", node_names_); get_parameter("autostart", autostart_); - startup_srv_ = create_service<std_srvs::srv::Empty>("lifecycle_manager/startup", - std::bind(&LifecycleManager::startupCallback, this, _1, _2, _3)); - - shutdown_srv_ = create_service<std_srvs::srv::Empty>("lifecycle_manager/shutdown", - std::bind(&LifecycleManager::shutdownCallback, this, _1, _2, _3)); + manager_srv_ = create_service<ManageNodes>("lifecycle_manager/manage_nodes", + std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3)); auto options = rclcpp::NodeOptions().arguments( {"--ros-args", std::string("__node:=") + get_name() + "service_client", "--"}); @@ -83,21 +81,29 @@ LifecycleManager::~LifecycleManager() } void -LifecycleManager::startupCallback( - const std::shared_ptr<rmw_request_id_t>/*request_header*/, - const std::shared_ptr<std_srvs::srv::Empty::Request>/*request*/, - std::shared_ptr<std_srvs::srv::Empty::Response>/*response*/) -{ - startup(); -} - -void -LifecycleManager::shutdownCallback( +LifecycleManager::managerCallback( const std::shared_ptr<rmw_request_id_t>/*request_header*/, - const std::shared_ptr<std_srvs::srv::Empty::Request>/*request*/, - std::shared_ptr<std_srvs::srv::Empty::Response>/*response*/) + const std::shared_ptr<ManageNodes::Request> request, + std::shared_ptr<ManageNodes::Response> response) { - shutdown(); + switch (request->command.id) + { + case Command::STARTUP : + response->success = startup(); + break; + case Command::RESET : + response->success = reset(); + break; + case Command::SHUTDOWN : + response->success = shutdown(); + break; + case Command::PAUSE : + response->success = pause(); + break; + case Command::RESUME : + response->success = resume(); + break; + } } void @@ -176,7 +182,7 @@ LifecycleManager::shutdownAllNodes() changeStateForAllNodes(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN, true); } -void +bool LifecycleManager::startup() { message("Starting the system bringup..."); @@ -185,18 +191,63 @@ LifecycleManager::startup() !changeStateForAllNodes(Transition::TRANSITION_ACTIVATE)) { RCLCPP_ERROR(get_logger(), "Failed to bring up nodes: aborting bringup"); - return; + return false; } message("The system is active"); + return true; } -void +bool LifecycleManager::shutdown() { message("Shutting down the system..."); shutdownAllNodes(); destroyLifecycleServiceClients(); message("The system has been sucessfully shut down"); + return true; +} + +bool +LifecycleManager::reset() +{ + message("Resetting the system..."); + createLifecycleServiceClients(); + if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE) || + !changeStateForAllNodes(Transition::TRANSITION_CLEANUP)) + { + RCLCPP_ERROR(get_logger(), "Failed to reset nodes: aborting reset"); + return false; + } + message("The system is reset"); + return true; +} + +bool +LifecycleManager::pause() +{ + message("Pausing the system..."); + createLifecycleServiceClients(); + if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE)) + { + RCLCPP_ERROR(get_logger(), "Failed to pause nodes: aborting pause"); + return false; + } + message("The system is paused"); + return true; +} + +bool +LifecycleManager::resume() +{ + message("Resuming the system..."); + createLifecycleServiceClients(); + if (!changeStateForAllNodes(Transition::TRANSITION_ACTIVATE)) + { + RCLCPP_ERROR(get_logger(), "Failed to resume nodes: aborting resume"); + return false; + } + message("The system is active"); + return true; } // TODO(mjeronimo): This is used to emphasize the major events during system bring-up and diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 5e5a03d974964ec64b42c1d19ca8d1b5b3bf935e..d35321bcb2249d1222245b2297cbc728edf94fa4 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -23,18 +23,15 @@ namespace nav2_lifecycle_manager { using nav2_util::geometry_utils::orientationAroundZAxis; +using nav2_msgs::msg::Command; LifecycleManagerClient::LifecycleManagerClient() { // Create the node to use for all of the service clients node_ = std::make_shared<rclcpp::Node>("lifecycle_manager_client_service_client"); - // All of the services use the same (Empty) request - request_ = std::make_shared<Empty::Request>(); - // Create the service clients - startup_client_ = node_->create_client<Empty>("lifecycle_manager/startup"); - shutdown_client_ = node_->create_client<Empty>("lifecycle_manager/shutdown"); + manager_client_ = node_->create_client<ManageNodes>("lifecycle_manager/manage_nodes"); navigate_action_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(node_, "NavigateToPose"); @@ -47,13 +44,13 @@ LifecycleManagerClient::LifecycleManagerClient() void LifecycleManagerClient::startup() { - callService(startup_client_, "lifecycle_manager/startup"); + callService(Command::STARTUP); } void LifecycleManagerClient::shutdown() { - callService(shutdown_client_, "lifecycle_manager/shutdown"); + callService(Command::SHUTDOWN); } void @@ -126,14 +123,16 @@ LifecycleManagerClient::navigate_to_pose(double x, double y, double theta) } void -LifecycleManagerClient::callService( - rclcpp::Client<Empty>::SharedPtr service_client, - const char * service_name) +LifecycleManagerClient::callService(uint8_t command) { + std::string service_name = "lifecycle_manager/manage_nodes"; + auto request = std::make_shared<ManageNodes::Request>(); + request->command.id = command; + RCLCPP_INFO(node_->get_logger(), "Waiting for the lifecycle_manager's %s service...", service_name); - while (!service_client->wait_for_service(std::chrono::seconds(1))) { + while (!manager_client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { RCLCPP_ERROR(node_->get_logger(), "Client interrupted while waiting for service to appear"); return; @@ -143,7 +142,7 @@ LifecycleManagerClient::callService( RCLCPP_INFO(node_->get_logger(), "send_async_request (%s) to the lifecycle_manager", service_name); - auto future_result = service_client->async_send_request(request_); + auto future_result = manager_client_->async_send_request(request); rclcpp::spin_until_future_complete(node_, future_result); } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 32421f69313f68dfec7d6baf1156393d611dfb57..f118d921953373734036361d78b981a43c4d29c1 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -16,10 +16,12 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/CostmapMetaData.msg" "msg/Path.msg" "msg/VoxelGrid.msg" + "msg/Command.msg" "srv/GetCostmap.srv" "srv/ClearCostmapExceptRegion.srv" "srv/ClearCostmapAroundRobot.srv" "srv/ClearEntireCostmap.srv" + "srv/ManageNodes.srv" "action/BackUp.action" "action/ComputePathToPose.action" "action/FollowPath.action" diff --git a/nav2_msgs/msg/Command.msg b/nav2_msgs/msg/Command.msg new file mode 100644 index 0000000000000000000000000000000000000000..7c9f871ccde26d2cb247bdca491bbfdb46ee7650 --- /dev/null +++ b/nav2_msgs/msg/Command.msg @@ -0,0 +1,7 @@ +uint8 STARTUP = 0 +uint8 PAUSE = 1 +uint8 RESUME = 2 +uint8 RESET = 3 +uint8 SHUTDOWN = 4 + +uint8 id diff --git a/nav2_msgs/srv/ManageNodes.srv b/nav2_msgs/srv/ManageNodes.srv new file mode 100644 index 0000000000000000000000000000000000000000..41c6cd3178c78d86d72690ae75a7ed4ddaa5e055 --- /dev/null +++ b/nav2_msgs/srv/ManageNodes.srv @@ -0,0 +1,4 @@ +Command command +--- +bool success +string reason