From fa9aaa935c5eae3dbcbd3f6a9652afbcde57f3f4 Mon Sep 17 00:00:00 2001 From: Saitama Date: Tue, 15 Oct 2024 19:55:35 +0200 Subject: [PATCH 1/4] [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1f9b8233e86ed54dfbe615b1ba56b51b6d added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart --- .../src/nav2_rotation_shim_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5947f5e36e..5b2bf306df 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -253,7 +253,10 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - return current_path_.poses.back(); + auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; + goal.header.stamp = clock_->now(); + return goal; } geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() From 682e08915d0c2cc3fa3ad1bac110dc72590732db Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Tue, 15 Oct 2024 23:32:08 +0200 Subject: [PATCH 2/4] 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 868c644e4b..f73f21952b 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 7daa327079..e43f718ebe 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 6c7fcbbad0..96cf7784bf 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; From c7ef3be69339bfa13ab76f74ecce5e7b70ae9b3f Mon Sep 17 00:00:00 2001 From: Daniil Khaninaev Date: Wed, 16 Oct 2024 23:39:12 +0300 Subject: [PATCH 3/4] Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev --- .../nav2_rotation_shim_controller.hpp | 9 ++- .../src/nav2_rotation_shim_controller.cpp | 18 +++++- .../test/test_shim_controller.cpp | 60 ++++++++++++++++++- 3 files changed, 84 insertions(+), 3 deletions(-) diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index d3807eeb95..abaf02c939 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -148,6 +148,13 @@ class RotationShimController : public nav2_core::Controller const double & angular_distance_to_heading, const geometry_msgs::msg::PoseStamped & pose); + /** + * @brief Checks if the goal has changed based on the given path. + * @param path The path to compare with the current goal. + * @return True if the goal has changed, false otherwise. + */ + bool isGoalChanged(const nav_msgs::msg::Path & path); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -171,7 +178,7 @@ class RotationShimController : public nav2_core::Controller double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; - bool rotate_to_goal_heading_, in_rotation_; + bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_; // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5b2bf306df..bb77879c7f 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -71,6 +71,8 @@ void RotationShimController::configure( node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_); @@ -86,6 +88,7 @@ void RotationShimController::configure( control_duration_ = 1.0 / control_frequency; node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_); try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); @@ -340,9 +343,20 @@ void RotationShimController::isCollisionFree( } } +bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path) +{ + // Return true if rotating or if the current path is empty + if (in_rotation_ || current_path_.poses.empty()) { + return true; + } + + // Check if the last pose of the current and new paths differ + return current_path_.poses.back().pose != path.poses.back().pose; +} + void RotationShimController::setPlan(const nav_msgs::msg::Path & path) { - path_updated_ = true; + path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true; current_path_ = path; primary_controller_->setPlan(path); } @@ -377,6 +391,8 @@ RotationShimController::dynamicParametersCallback(std::vector } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".rotate_to_goal_heading") { rotate_to_goal_heading_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".rotate_to_heading_once") { + rotate_to_heading_once_ = parameter.as_bool(); } } } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1160a5a98a..1d63a77b47 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -62,6 +62,11 @@ class RotationShimShim : public nav2_rotation_shim_controller::RotationShimContr return getSampledPathPt(); } + bool isGoalChangedWrapper(const nav_msgs::msg::Path & path) + { + return isGoalChanged(path); + } + geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(geometry_msgs::msg::PoseStamped pt) { return transformPoseToBaseFrame(pt); @@ -382,6 +387,57 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } +TEST(RotationShimControllerTest, isGoalChangedTest) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "base_link"; + transform.child_frame_id = "odom"; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + tf_broadcaster->sendTransform(transform); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter( + "PathFollower.rotate_to_heading_once", + true); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + nav_msgs::msg::Path path; + path.header.frame_id = "base_link"; + path.poses.resize(2); + path.poses.back().pose.position.x = 2.0; + path.poses.back().pose.position.y = 2.0; + + // Test: Current path is empty, should return true + EXPECT_EQ(controller->isGoalChangedWrapper(path), true); + + // Test: Last pose of the current path is the same, should return false + controller->setPlan(path); + EXPECT_EQ(controller->isGoalChangedWrapper(path), false); + + // Test: Last pose of the current path differs, should return true + path.poses.back().pose.position.x = 3.0; + EXPECT_EQ(controller->isGoalChangedWrapper(path), true); +} + TEST(RotationShimControllerTest, testDynamicParameter) { auto node = std::make_shared("ShimControllerTest"); @@ -412,7 +468,8 @@ TEST(RotationShimControllerTest, testDynamicParameter) rclcpp::Parameter("test.max_angular_accel", 7.0), rclcpp::Parameter("test.simulate_ahead_time", 7.0), rclcpp::Parameter("test.primary_controller", std::string("HI")), - rclcpp::Parameter("test.rotate_to_goal_heading", true)}); + rclcpp::Parameter("test.rotate_to_goal_heading", true), + rclcpp::Parameter("test.rotate_to_heading_once", true)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -424,4 +481,5 @@ TEST(RotationShimControllerTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true); } From 44a69ccde93514ed1a962b7f18cb101ec671d93a Mon Sep 17 00:00:00 2001 From: Saitama Date: Thu, 17 Oct 2024 18:16:50 +0200 Subject: [PATCH 4/4] [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart Co-authored-by: agennart --- .../src/nav2_rotation_shim_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index bb77879c7f..f5fcf4c5b2 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -269,6 +269,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() } auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; goal.header.stamp = clock_->now(); return goal; }