From 56b497570ea8987647b467741c6981719076fe21 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Tue, 15 Oct 2024 23:32:08 +0200 Subject: [PATCH] Improve Docking panel (#4717) * Added load and save panel Signed-off-by: Alberto Tudela * Improved dock_panel state machine Signed-off-by: Alberto Tudela * Added loading dock plugins log Signed-off-by: Alberto Tudela * Redo UI Signed-off-by: Alberto Tudela * Update tooltips Signed-off-by: Alberto Tudela * Fix null-dereference Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela --- .../nav2_rviz_plugins/docking_panel.hpp | 110 ++++-- nav2_rviz_plugins/src/docking_panel.cpp | 315 +++++++++++++----- nav2_rviz_plugins/src/utils.cpp | 4 +- 3 files changed, 317 insertions(+), 112 deletions(-) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index 868c644e4b1..f73f21952be 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -23,6 +23,7 @@ #include // ROS +#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rviz_common/panel.hpp" @@ -37,6 +38,9 @@ class QPushButton; namespace nav2_rviz_plugins { +class InitialDockThread; + +/// Panel to interface to the docking server class DockingPanel : public rviz_common::Panel { Q_OBJECT @@ -44,11 +48,20 @@ class DockingPanel : public rviz_common::Panel public: explicit DockingPanel(QWidget * parent = 0); virtual ~DockingPanel(); + void onInitialize() override; + /// Load and save configuration data + void load(const rviz_common::Config & config) override; + void save(rviz_common::Config config) const override; + private Q_SLOTS: + void startThread(); + void onStartup(); void onDockingButtonPressed(); void onUndockingButtonPressed(); + void onCancelDocking(); + void onCancelUndocking(); void dockIdCheckbox(); private: @@ -57,14 +70,6 @@ private Q_SLOTS: using DockGoalHandle = rclcpp_action::ClientGoalHandle; using UndockGoalHandle = rclcpp_action::ClientGoalHandle; - // Start the actions - void startDocking(); - void startUndocking(); - - // Cancel the actions - void cancelDocking(); - void cancelUndocking(); - // The (non-spinning) client node used to invoke the action client void timerEvent(QTimerEvent * event) override; @@ -84,14 +89,33 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + + // The Node pointer that we need to keep alive for the duration of this plugin. + std::shared_ptr node_ptr_; + // Timeout value when waiting for action servers to respond std::chrono::milliseconds server_timeout_; + // A timer used to check on the completion status of the action + QBasicTimer action_timer_; + + // The Dock and Undock action client + rclcpp_action::Client::SharedPtr dock_client_; + rclcpp_action::Client::SharedPtr undock_client_; + + // Docking / Undocking action feedback subscribers + rclcpp::Subscription::SharedPtr docking_feedback_sub_; + rclcpp::Subscription::SharedPtr undocking_feedback_sub_; + rclcpp::Subscription::SharedPtr docking_goal_status_sub_; + rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; + + // Goal related state + DockGoalHandle::SharedPtr dock_goal_handle_; + UndockGoalHandle::SharedPtr undock_goal_handle_; + // Flags to indicate if the plugins have been loaded bool plugins_loaded_ = false; bool server_failed_ = false; - bool tried_once_ = false; - QBasicTimer timer_; QVBoxLayout * main_layout_{nullptr}; QHBoxLayout * info_layout_{nullptr}; @@ -121,20 +145,62 @@ private Q_SLOTS: bool undocking_in_progress_ = false; bool use_dock_id_ = false; - // The Dock and Undock action client - rclcpp_action::Client::SharedPtr dock_client_; - rclcpp_action::Client::SharedPtr undock_client_; - DockGoalHandle::SharedPtr dock_goal_handle_; - UndockGoalHandle::SharedPtr undock_goal_handle_; + QStateMachine state_machine_; + InitialDockThread * initial_thread_; - // The Node pointer that we need to keep alive for the duration of this plugin. - std::shared_ptr node_ptr_; + QState * pre_initial_{nullptr}; + QState * idle_{nullptr}; + QState * docking_{nullptr}; + QState * undocking_{nullptr}; + QState * canceled_docking_{nullptr}; + QState * canceled_undocking_{nullptr}; +}; - // Docking / Undocking action feedback subscribers - rclcpp::Subscription::SharedPtr docking_feedback_sub_; - rclcpp::Subscription::SharedPtr undocking_feedback_sub_; - rclcpp::Subscription::SharedPtr docking_goal_status_sub_; - rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; +class InitialDockThread : public QThread +{ + Q_OBJECT + +public: + explicit InitialDockThread( + rclcpp_action::Client::SharedPtr & dock_client, + rclcpp_action::Client::SharedPtr & undock_client) + : dock_client_(dock_client), undock_client_(undock_client) + {} + + void run() override + { + while (!dock_active_) { + dock_active_ = dock_client_->wait_for_action_server(std::chrono::seconds(1)); + } + + while (!undock_active_) { + undock_active_ = undock_client_->wait_for_action_server(std::chrono::seconds(1)); + } + + if (dock_active_) { + emit dockingActive(); + } else { + emit dockingInactive(); + } + + if (undock_active_) { + emit undockingActive(); + } else { + emit undockingInactive(); + } + } + +signals: + void dockingActive(); + void dockingInactive(); + void undockingActive(); + void undockingInactive(); + +private: + rclcpp_action::Client::SharedPtr dock_client_; + rclcpp_action::Client::SharedPtr undock_client_; + bool dock_active_ = false; + bool undock_active_ = false; }; } // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 7daa327079c..e43f718ebee 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -29,6 +29,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_rviz_plugins/docking_panel.hpp" +#include "nav2_rviz_plugins/ros_action_qevent.hpp" #include "nav2_rviz_plugins/utils.hpp" using namespace std::chrono_literals; @@ -40,8 +41,7 @@ DockingPanel::DockingPanel(QWidget * parent) : Panel(parent), server_timeout_(100) { - client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); - + // Create the control buttons and its tooltip main_layout_ = new QVBoxLayout; info_layout_ = new QHBoxLayout; feedback_layout_ = new QVBoxLayout; @@ -50,8 +50,8 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_layout_ = new QHBoxLayout; nav_stage_layout_ = new QHBoxLayout; dock_type_ = new QComboBox; - docking_button_ = new QPushButton("Dock robot"); - undocking_button_ = new QPushButton("Undock robot"); + docking_button_ = new QPushButton; + undocking_button_ = new QPushButton; docking_goal_status_indicator_ = new QLabel; docking_feedback_indicator_ = new QLabel; docking_result_indicator_ = new QLabel; @@ -62,27 +62,147 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_y_ = new QLineEdit; dock_pose_yaw_ = new QLineEdit; - docking_button_->setEnabled(false); - undocking_button_->setEnabled(false); - use_dock_id_checkbox_->setEnabled(false); - nav_to_staging_checkbox_->setEnabled(false); - dock_id_->setEnabled(false); - dock_pose_x_->setEnabled(false); - dock_pose_y_->setEnabled(false); - dock_pose_yaw_->setEnabled(false); + // Create the state machine used to present the proper control button states in the UI + const char * nav_to_stage_msg = "Navigate to the staging pose before docking"; + const char * use_dock_id_msg = "Use the dock id or the dock pose to dock the robot"; + const char * dock_msg = "Dock the robot at the specified docking station"; + const char * undock_msg = "Undock the robot from the docking station"; + const char * cancel_dock_msg = "Cancel the current docking action"; + const char * cancel_undock_msg = "Cancel the current undocking action"; + docking_goal_status_indicator_->setText(nav2_rviz_plugins::getGoalStatusLabel()); docking_feedback_indicator_->setText(getDockFeedbackLabel()); docking_goal_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); docking_feedback_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); - nav_to_staging_checkbox_->setFixedWidth(150); + pre_initial_ = new QState(); + pre_initial_->setObjectName("pre_initial"); + pre_initial_->assignProperty(docking_button_, "text", "Dock robot"); + pre_initial_->assignProperty(docking_button_, "enabled", false); + + pre_initial_->assignProperty(undocking_button_, "text", "Undock robot"); + pre_initial_->assignProperty(undocking_button_, "enabled", false); + + pre_initial_->assignProperty(nav_to_staging_checkbox_, "enabled", false); + pre_initial_->assignProperty(nav_to_staging_checkbox_, "checked", true); + pre_initial_->assignProperty(use_dock_id_checkbox_, "enabled", false); + pre_initial_->assignProperty(use_dock_id_checkbox_, "checked", true); + pre_initial_->assignProperty(dock_id_, "enabled", false); + pre_initial_->assignProperty(dock_type_, "enabled", false); + pre_initial_->assignProperty(dock_pose_x_, "enabled", false); + pre_initial_->assignProperty(dock_pose_y_, "enabled", false); + pre_initial_->assignProperty(dock_pose_yaw_, "enabled", false); + + // State entered when the docking / undocking action is not active + idle_ = new QState(); + idle_->setObjectName("idle"); + idle_->assignProperty(docking_button_, "text", "Dock robot"); + idle_->assignProperty(docking_button_, "toolTip", dock_msg); + idle_->assignProperty(docking_button_, "enabled", true); + + idle_->assignProperty(undocking_button_, "text", "Undock robot"); + idle_->assignProperty(undocking_button_, "toolTip", undock_msg); + idle_->assignProperty(undocking_button_, "enabled", true); + + idle_->assignProperty(nav_to_staging_checkbox_, "enabled", true); + idle_->assignProperty(nav_to_staging_checkbox_, "toolTip", nav_to_stage_msg); + idle_->assignProperty(use_dock_id_checkbox_, "enabled", true); + idle_->assignProperty(use_dock_id_checkbox_, "toolTip", use_dock_id_msg); + idle_->assignProperty(dock_id_, "enabled", true); + idle_->assignProperty(dock_type_, "enabled", true); + + // State entered to cancel the docking action + canceled_docking_ = new QState(); + canceled_docking_->setObjectName("canceled_docking"); + + // State entered to cancel the undocking action + canceled_undocking_ = new QState(); + canceled_undocking_->setObjectName("canceled_undocking"); + + // State entered while the docking action is active + docking_ = new QState(); + docking_->setObjectName("docking"); + docking_->assignProperty(docking_button_, "text", "Cancel docking"); + docking_->assignProperty(docking_button_, "toolTip", cancel_dock_msg); + + docking_->assignProperty(undocking_button_, "enabled", false); + + // State entered while the undocking action is active + undocking_ = new QState(); + undocking_->setObjectName("undocking"); + undocking_->assignProperty(docking_button_, "enabled", false); + + undocking_->assignProperty(undocking_button_, "text", "Cancel undocking"); + undocking_->assignProperty(undocking_button_, "toolTip", cancel_undock_msg); + + QObject::connect(docking_, SIGNAL(entered()), this, SLOT(onDockingButtonPressed())); + QObject::connect(undocking_, SIGNAL(entered()), this, SLOT(onUndockingButtonPressed())); + QObject::connect(canceled_docking_, SIGNAL(exited()), this, SLOT(onCancelDocking())); + QObject::connect(canceled_undocking_, SIGNAL(exited()), this, SLOT(onCancelUndocking())); + + // Start/Cancel button click transitions + idle_->addTransition(docking_button_, SIGNAL(clicked()), docking_); + idle_->addTransition(undocking_button_, SIGNAL(clicked()), undocking_); + docking_->addTransition(docking_button_, SIGNAL(clicked()), canceled_docking_); + undocking_->addTransition(undocking_button_, SIGNAL(clicked()), canceled_undocking_); + + // Internal state transitions + canceled_docking_->addTransition(canceled_docking_, SIGNAL(entered()), idle_); + canceled_undocking_->addTransition(canceled_undocking_, SIGNAL(entered()), idle_); + + // ROSAction Transitions: So when actions are updated remotely (failing, succeeding, etc) + // the state of the application will also update. This means that if in the processing + // states and then goes inactive, move back to the idle state. Vise versa as well. + ROSActionQTransition * idleDockTransition = new ROSActionQTransition(QActionState::INACTIVE); + idleDockTransition->setTargetState(docking_); + idle_->addTransition(idleDockTransition); + + ROSActionQTransition * idleUndockTransition = new ROSActionQTransition(QActionState::INACTIVE); + idleUndockTransition->setTargetState(undocking_); + idle_->addTransition(idleUndockTransition); + + ROSActionQTransition * dockingTransition = new ROSActionQTransition(QActionState::ACTIVE); + dockingTransition->setTargetState(idle_); + docking_->addTransition(dockingTransition); + + ROSActionQTransition * undockingTransition = new ROSActionQTransition(QActionState::ACTIVE); + undockingTransition->setTargetState(idle_); + undocking_->addTransition(undockingTransition); + + client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); + + state_machine_.addState(pre_initial_); + state_machine_.addState(idle_); + state_machine_.addState(docking_); + state_machine_.addState(undocking_); + state_machine_.addState(canceled_docking_); + state_machine_.addState(canceled_undocking_); + + state_machine_.setInitialState(pre_initial_); + + // Delay starting initial thread until state machine has started or a race occurs + QObject::connect(&state_machine_, SIGNAL(started()), this, SLOT(startThread())); + state_machine_.start(); + + // Create the layout for the panel info_layout_->addWidget(docking_goal_status_indicator_); info_layout_->addWidget(docking_result_indicator_); feedback_layout_->addWidget(docking_feedback_indicator_); - dock_id_layout_->addWidget(new QLabel("Dock id")); + + QLabel * nav_stage_label = new QLabel("Nav. to staging pose"); + QLabel * dock_id_label = new QLabel("Dock id"); + QLabel * dock_type_label = new QLabel("Dock type"); + + nav_stage_label->setFixedWidth(150); + dock_id_label->setFixedWidth(150); + dock_type_label->setFixedWidth(170); + + nav_stage_layout_->addWidget(nav_stage_label); + nav_stage_layout_->addWidget(nav_to_staging_checkbox_); + dock_id_layout_->addWidget(dock_id_label); dock_id_layout_->addWidget(use_dock_id_checkbox_); dock_id_layout_->addWidget(dock_id_); - dock_type_layout_->addWidget(new QLabel("Dock type")); + dock_type_layout_->addWidget(dock_type_label); dock_type_layout_->addWidget(dock_type_); dock_pose_layout_->addWidget(new QLabel("Dock pose {X")); dock_pose_layout_->addWidget(dock_pose_x_); @@ -91,28 +211,53 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_layout_->addWidget(new QLabel("θ")); dock_pose_layout_->addWidget(dock_pose_yaw_); dock_pose_layout_->addWidget(new QLabel("}")); - nav_stage_layout_->addWidget(nav_to_staging_checkbox_); - nav_stage_layout_->addWidget(new QLabel("Navigate to staging pose")); + + QGroupBox * group_box = new QGroupBox(); + QVBoxLayout * group_box_layout = new QVBoxLayout; + group_box_layout->addLayout(nav_stage_layout_); + group_box_layout->addLayout(dock_id_layout_); + group_box_layout->addLayout(dock_type_layout_); + group_box_layout->addLayout(dock_pose_layout_); + group_box->setLayout(group_box_layout); main_layout_->setContentsMargins(10, 10, 10, 10); main_layout_->addLayout(info_layout_); main_layout_->addLayout(feedback_layout_); - main_layout_->addLayout(nav_stage_layout_); - main_layout_->addLayout(dock_id_layout_); - main_layout_->addLayout(dock_type_layout_); - main_layout_->addLayout(dock_pose_layout_); + main_layout_->addWidget(group_box); main_layout_->addWidget(docking_button_); main_layout_->addWidget(undocking_button_); setLayout(main_layout_); - timer_.start(200, this); + action_timer_.start(200, this); dock_client_ = rclcpp_action::create_client(client_node_, "dock_robot"); undock_client_ = rclcpp_action::create_client(client_node_, "undock_robot"); + initial_thread_ = new InitialDockThread(dock_client_, undock_client_); + connect(initial_thread_, &InitialDockThread::finished, initial_thread_, &QObject::deleteLater); + + QSignalTransition * activeDockSignal = new QSignalTransition( + initial_thread_, &InitialDockThread::dockingActive); + activeDockSignal->setTargetState(idle_); + pre_initial_->addTransition(activeDockSignal); + + QSignalTransition * activeUndockSignal = new QSignalTransition( + initial_thread_, &InitialDockThread::undockingActive); + activeUndockSignal->setTargetState(idle_); + pre_initial_->addTransition(activeUndockSignal); + + QObject::connect( + initial_thread_, &InitialDockThread::dockingActive, + [this] { + // Load the plugins if not already loaded + if (!plugins_loaded_) { + RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + plugins_loaded_ = true; + } + }); // Conect buttons with functions - QObject::connect(docking_button_, SIGNAL(clicked()), this, SLOT(onDockingButtonPressed())); - QObject::connect(undocking_button_, SIGNAL(clicked()), this, SLOT(onUndockingButtonPressed())); QObject::connect( use_dock_id_checkbox_, &QCheckBox::stateChanged, this, &DockingPanel::dockIdCheckbox); } @@ -135,18 +280,6 @@ void DockingPanel::onInitialize() rclcpp::SystemDefaultsQoS(), [this](const Dock::Impl::FeedbackMessage::SharedPtr msg) { docking_feedback_indicator_->setText(getDockFeedbackLabel(msg->feedback)); - docking_button_->setText("Cancel docking"); - undocking_button_->setEnabled(false); - docking_in_progress_ = true; - }); - - undocking_feedback_sub_ = node->create_subscription( - "undock_robot/_action/feedback", - rclcpp::SystemDefaultsQoS(), - [this](const Undock::Impl::FeedbackMessage::SharedPtr /*msg*/) { - docking_button_->setEnabled(false); - undocking_button_->setText("Cancel undocking"); - undocking_in_progress_ = true; }); // Create action goal status subscribers @@ -156,11 +289,6 @@ void DockingPanel::onInitialize() [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - docking_button_->setText("Dock robot"); - undocking_button_->setEnabled(true); - docking_in_progress_ = false; - } // Reset values when action is completed if (msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { docking_feedback_indicator_->setText(getDockFeedbackLabel()); @@ -173,37 +301,30 @@ void DockingPanel::onInitialize() [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - docking_button_->setEnabled(true); - undocking_button_->setText("Undock robot"); - undocking_in_progress_ = false; - } }); } +void DockingPanel::startThread() +{ + // start initial thread now that state machine is started + initial_thread_->start(); +} + DockingPanel::~DockingPanel() { } -void DockingPanel::onDockingButtonPressed() +void DockingPanel::load(const rviz_common::Config & config) { - if (!docking_in_progress_) { - startDocking(); - } else { - cancelDocking(); - } + Panel::load(config); } -void DockingPanel::onUndockingButtonPressed() +void DockingPanel::save(rviz_common::Config config) const { - if (!undocking_in_progress_) { - startUndocking(); - } else { - cancelUndocking(); - } + Panel::save(config); } -void DockingPanel::startDocking() +void DockingPanel::onDockingButtonPressed() { auto is_action_server_ready = dock_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -286,10 +407,10 @@ void DockingPanel::startDocking() return; } - timer_.start(200, this); + action_timer_.start(200, this); } -void DockingPanel::startUndocking() +void DockingPanel::onUndockingButtonPressed() { auto is_action_server_ready = undock_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -344,7 +465,7 @@ void DockingPanel::startUndocking() return; } - timer_.start(200, this); + action_timer_.start(200, this); } void DockingPanel::dockIdCheckbox() @@ -364,7 +485,7 @@ void DockingPanel::dockIdCheckbox() } } -void DockingPanel::cancelDocking() +void DockingPanel::onCancelDocking() { if (dock_goal_handle_) { auto future_cancel = dock_client_->async_cancel_goal(dock_goal_handle_); @@ -377,9 +498,11 @@ void DockingPanel::cancelDocking() dock_goal_handle_.reset(); } } + + action_timer_.stop(); } -void DockingPanel::cancelUndocking() +void DockingPanel::onCancelUndocking() { if (undock_goal_handle_) { auto future_cancel = undock_client_->async_cancel_goal(undock_goal_handle_); @@ -392,35 +515,53 @@ void DockingPanel::cancelUndocking() undock_goal_handle_.reset(); } } + + action_timer_.stop(); } void DockingPanel::timerEvent(QTimerEvent * event) { - if (event->timerId() == timer_.timerId()) { - if (!plugins_loaded_) { - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); - plugins_loaded_ = true; - docking_button_->setEnabled(true); - undocking_button_->setEnabled(true); - use_dock_id_checkbox_->setEnabled(true); - use_dock_id_checkbox_->setChecked(true); - nav_to_staging_checkbox_->setEnabled(true); - nav_to_staging_checkbox_->setChecked(true); - dock_id_->setEnabled(true); - } + if (event->timerId() == action_timer_.timerId()) { + // Check the status of the action servers + if (state_machine_.configuration().contains(docking_)) { + if (!dock_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } - // Restart the timer if the one of the server fails - if (server_failed_ && !tried_once_) { - RCLCPP_INFO(client_node_->get_logger(), "Retrying to connect to the failed server."); - server_failed_ = false; - plugins_loaded_ = false; - tried_once_ = true; - timer_.start(200, this); - return; - } + rclcpp::spin_some(client_node_); + auto status = dock_goal_handle_->get_status(); - timer_.stop(); + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + action_timer_.stop(); + } + } else if (state_machine_.configuration().contains(undocking_)) { + if (!undock_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } + + rclcpp::spin_some(client_node_); + auto status = undock_goal_handle_->get_status(); + + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + action_timer_.stop(); + } + } } } diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index 6c7fcbbad03..96cf7784bf1 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -38,9 +38,7 @@ void pluginLoader( RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); rclcpp::shutdown(); } - RCLCPP_INFO( - node->get_logger(), - "%s service not available", server_name.c_str()); + RCLCPP_INFO(node->get_logger(), "%s service not available", server_name.c_str()); server_unavailable = true; server_failed = true; break;