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 74035a8bfd3dae106c90b44fae1d8dfa14151ba9..d5a89d202cbd00a4ac164c7bebdeec2d5e9de49f 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -25,6 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/empty.hpp" #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp" +#include "std_srvs/srv/trigger.hpp" namespace nav2_lifecycle_manager { @@ -43,12 +44,18 @@ protected: // The services provided by this node rclcpp::Service<ManageLifecycleNodes>::SharedPtr manager_srv_; + rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr is_active_srv_; void managerCallback( const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<ManageLifecycleNodes::Request> request, std::shared_ptr<ManageLifecycleNodes::Response> response); + void activeCallback( + const std::shared_ptr<rmw_request_id_t> request_header, + const std::shared_ptr<std_srvs::srv::Trigger::Request> request, + std::shared_ptr<std_srvs::srv::Trigger::Response> response); + // Support functions for the service calls bool startup(); bool shutdown(); @@ -85,6 +92,8 @@ protected: // Whether to automatically start up the system bool autostart_; + + bool system_active_{false}; }; } // namespace nav2_lifecycle_manager 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 cdc1aba8296cff7dd0cbde128ecb4d03e883ea4a..677c6b849a6aac85b9f7795cd9495d6ef26e2cd8 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 @@ -25,6 +25,7 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "std_srvs/srv/empty.hpp" #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp" +#include "std_srvs/srv/trigger.hpp" namespace nav2_lifecycle_manager { @@ -40,6 +41,7 @@ public: bool pause(); bool resume(); bool reset(); + bool is_active(); // A couple convenience methods to facilitate scripting tests void set_initial_pose(double x, double y, double theta); @@ -55,6 +57,7 @@ protected: rclcpp::Node::SharedPtr node_; rclcpp::Client<ManageLifecycleNodes>::SharedPtr manager_client_; + rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr is_active_client_; std::string service_name_{"lifecycle_manager/manage_nodes"}; 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 e4ce91979d5055a47b115b6ded5e5c05e3365f9f..83cfeabdedb62e43b8b71b1ad0caa5ce2601721d 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -51,6 +51,9 @@ LifecycleManager::LifecycleManager() manager_srv_ = create_service<ManageLifecycleNodes>("lifecycle_manager/manage_nodes", std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3)); + is_active_srv_ = create_service<std_srvs::srv::Trigger>("lifecycle_manager/is_active", + std::bind(&LifecycleManager::activeCallback, this, _1, _2, _3)); + auto options = rclcpp::NodeOptions().arguments( {"--ros-args", "-r", std::string("__node:=") + get_name() + "service_client", "--"}); service_client_node_ = std::make_shared<rclcpp::Node>("_", options); @@ -106,6 +109,15 @@ LifecycleManager::managerCallback( } } +void +LifecycleManager::activeCallback( + const std::shared_ptr<rmw_request_id_t>/*request_header*/, + const std::shared_ptr<std_srvs::srv::Trigger::Request> /*request*/, + std::shared_ptr<std_srvs::srv::Trigger::Response> response) +{ + response->success = system_active_; +} + void LifecycleManager::createLifecycleServiceClients() { @@ -180,6 +192,7 @@ LifecycleManager::startup() return false; } message("Managed nodes are active"); + system_active_ = true; return true; } @@ -190,6 +203,7 @@ LifecycleManager::shutdown() shutdownAllNodes(); destroyLifecycleServiceClients(); message("Managed nodes have been shut down"); + system_active_ = false; return true; } @@ -204,6 +218,7 @@ LifecycleManager::reset() return false; } message("Managed nodes have been reset"); + system_active_ = false; return true; } @@ -216,6 +231,7 @@ LifecycleManager::pause() return false; } message("Managed nodes have been paused"); + system_active_ = false; return true; } @@ -228,6 +244,7 @@ LifecycleManager::resume() return false; } message("Managed nodes are active"); + system_active_ = true; return true; } diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index b6696d27fb27d60c6a18045b7881e655c9553aba..c2d2ea9e9df6b364fa30d151d2935220a793724f 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -31,6 +31,7 @@ LifecycleManagerClient::LifecycleManagerClient() // Create the service clients manager_client_ = node_->create_client<ManageLifecycleNodes>(service_name_); + is_active_client_ = node_->create_client<std_srvs::srv::Trigger>("lifecycle_manager/is_active"); navigate_action_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(node_, "NavigateToPose"); @@ -70,6 +71,29 @@ LifecycleManagerClient::reset() return callService(ManageLifecycleNodes::Request::RESET); } +bool LifecycleManagerClient::is_active() +{ + auto request = std::make_shared<std_srvs::srv::Trigger::Request>(); + auto service_name = "lifecycle_manager/is_active"; + + RCLCPP_INFO(node_->get_logger(), "Waiting for the lifecycle_manager's %s service...", + service_name); + + while (!is_active_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 false; + } + RCLCPP_INFO(node_->get_logger(), "Waiting for service to appear..."); + } + + RCLCPP_INFO(node_->get_logger(), "send_async_request (%s) to the lifecycle_manager", + service_name); + auto future_result = is_active_client_->async_send_request(request); + rclcpp::spin_until_future_complete(node_, future_result); + return future_result.get()->success; +} + void LifecycleManagerClient::set_initial_pose(double x, double y, double theta) { diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 4dc5900b27ada69de7a79ec6d50df54d0f46807c..d152420d99cac334cb0026e24940deee7fd8a186 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -87,6 +87,7 @@ private: QStateMachine state_machine_; + QState * pre_initial_{nullptr}; QState * initial_{nullptr}; QState * idle_{nullptr}; QState * stopping_{nullptr}; @@ -100,6 +101,32 @@ private: QState * canceled_{nullptr}; }; +class InitialThread : public QThread +{ + Q_OBJECT +public: + InitialThread(nav2_lifecycle_manager::LifecycleManagerClient & client) + : client_(client) + {} + + void run() override { + auto response = client_.is_active(); + if (response) { + emit activeSystem(); + } else { + emit inactiveSystem(); + } + } + +signals: + void activeSystem(); + void inactiveSystem(); + +private: + nav2_lifecycle_manager::LifecycleManagerClient client_; +}; + + } // namespace nav2_rviz_plugins #endif // NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_ diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 9a9593bfb9a87c92bd816e3faace78c40c1fd6a6..2118d011b8c084613375cfc0cf54a5d6750093b3 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -44,10 +44,16 @@ Nav2Panel::Nav2Panel(QWidget * parent) const char * shutdown_msg = "Deactivate, cleanup, and shutdown all nav2 lifecycle nodes"; const char * cancel_msg = "Cancel navigation"; + pre_initial_ = new QState(); + pre_initial_->setObjectName("pre_initial"); + pre_initial_->assignProperty(start_stop_button_, "text", "Startup"); + pre_initial_->assignProperty(start_stop_button_, "enabled", false); + initial_ = new QState(); initial_->setObjectName("initial"); initial_->assignProperty(start_stop_button_, "text", "Startup"); initial_->assignProperty(start_stop_button_, "toolTip", startup_msg); + initial_->assignProperty(start_stop_button_, "enabled", true); // State entered when NavigateToPose is not active idle_ = new QState(); @@ -87,13 +93,25 @@ Nav2Panel::Nav2Panel(QWidget * parent) runningTransition->setTargetState(idle_); running_->addTransition(runningTransition); + InitialThread * initialThread = new InitialThread(client_); + connect(initialThread, &InitialThread::finished, initialThread, &QObject::deleteLater); + + QSignalTransition * activeSignal = new QSignalTransition(initialThread, &InitialThread::activeSystem); + activeSignal->setTargetState(idle_); + pre_initial_->addTransition(activeSignal); + + QSignalTransition * inactiveSignal = new QSignalTransition(initialThread, &InitialThread::inactiveSystem); + inactiveSignal->setTargetState(initial_); + pre_initial_->addTransition(inactiveSignal); + + state_machine_.addState(pre_initial_); state_machine_.addState(initial_); state_machine_.addState(idle_); state_machine_.addState(stopping_); state_machine_.addState(running_); state_machine_.addState(canceled_); - state_machine_.setInitialState(initial_); + state_machine_.setInitialState(pre_initial_); state_machine_.start(); // Lay out the items in the panel @@ -112,6 +130,8 @@ Nav2Panel::Nav2Panel(QWidget * parent) QObject::connect(&GoalUpdater, SIGNAL(updateGoal(double,double,double,QString)), // NOLINT this, SLOT(onNewGoal(double,double,double,QString))); // NOLINT + + initialThread->start(); } Nav2Panel::~Nav2Panel()