From 3b911dc3031dbe29f38f01f73a081e2906bb0ce9 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Fri, 28 Jun 2024 21:03:48 +0200 Subject: [PATCH] Dock panel (#4458) * Initial docking panel Signed-off-by: Alberto Tudela * Only one goal status Signed-off-by: Alberto Tudela * Added dock pose Signed-off-by: Alberto Tudela * Fix size of text Signed-off-by: Alberto Tudela * Update rviz Signed-off-by: Alberto Tudela * Update rviz config Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela --- nav2_bringup/rviz/nav2_default_view.rviz | 22 +- nav2_rviz_plugins/CMakeLists.txt | 9 + nav2_rviz_plugins/icons/classes/Docking.svg | 18 + .../nav2_rviz_plugins/docking_panel.hpp | 137 +++++ .../include/nav2_rviz_plugins/nav2_panel.hpp | 4 - .../include/nav2_rviz_plugins/selector.hpp | 13 - .../include/nav2_rviz_plugins/utils.hpp | 46 ++ nav2_rviz_plugins/package.xml | 2 +- nav2_rviz_plugins/plugins_description.xml | 6 + nav2_rviz_plugins/src/docking_panel.cpp | 486 ++++++++++++++++++ nav2_rviz_plugins/src/nav2_panel.cpp | 56 +- nav2_rviz_plugins/src/selector.cpp | 62 +-- nav2_rviz_plugins/src/utils.cpp | 97 ++++ 13 files changed, 836 insertions(+), 122 deletions(-) create mode 100644 nav2_rviz_plugins/icons/classes/Docking.svg create mode 100644 nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp create mode 100644 nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp create mode 100644 nav2_rviz_plugins/src/docking_panel.cpp create mode 100644 nav2_rviz_plugins/src/utils.cpp diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index 3b64a69f06..01116aa7ce 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -8,7 +8,7 @@ Panels: - /TF1/Frames1 - /TF1/Tree1 Splitter Ratio: 0.5833333134651184 - Tree Height: 606 + Tree Height: 406 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -25,6 +25,8 @@ Panels: Name: Navigation 2 - Class: nav2_rviz_plugins/Selector Name: Selector + - Class: nav2_rviz_plugins/Docking + Name: Docking Visualization Manager: Class: "" Displays: @@ -581,20 +583,24 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 932 + Docking: + collapsed: false + Height: 1043 Hide Left Dock: false - Hide Right Dock: true + Hide Right Dock: false Navigation 2: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002de000000a90000008100fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a000003b9fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006900730070006c006100790073010000003d000001d3000000c900fffffffb00000018004e0061007600690067006100740069006f006e002000320100000216000001e0000001e000fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000015b000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000001b5000000a400fffffffb0000000e0044006f0063006b0069006e006701000001f8000001320000013200fffffffb0000001000530065006c006500630074006f00720100000330000000c6000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000469000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RealsenseCamera: collapsed: false Selection: collapsed: false + Selector: + collapsed: false Tool Properties: collapsed: false Views: - collapsed: true - Width: 1545 - X: 696 - Y: 229 + collapsed: false + Width: 1850 + X: 1990 + Y: 0 diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index 9bf25cd952..ef8158d49f 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -34,11 +34,13 @@ find_package(visualization_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) set(nav2_rviz_plugins_headers_to_moc + include/nav2_rviz_plugins/docking_panel.hpp include/nav2_rviz_plugins/goal_pose_updater.hpp include/nav2_rviz_plugins/goal_common.hpp include/nav2_rviz_plugins/goal_tool.hpp include/nav2_rviz_plugins/nav2_panel.hpp include/nav2_rviz_plugins/selector.hpp + include/nav2_rviz_plugins/utils.hpp include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp ) @@ -50,9 +52,11 @@ include_directories( set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED + src/docking_panel.cpp src/goal_tool.cpp src/nav2_panel.cpp src/selector.cpp + src/utils.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp ${nav2_rviz_plugins_headers_to_moc} @@ -111,6 +115,11 @@ install( DESTINATION include/ ) +install( + DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION "share/${PROJECT_NAME}" +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nav2_rviz_plugins/icons/classes/Docking.svg b/nav2_rviz_plugins/icons/classes/Docking.svg new file mode 100644 index 0000000000..743f7d3598 --- /dev/null +++ b/nav2_rviz_plugins/icons/classes/Docking.svg @@ -0,0 +1,18 @@ + + + + + + + + + image/svg+xml + + + + + + + + + \ No newline at end of file diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp new file mode 100644 index 0000000000..68380cb43e --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -0,0 +1,137 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_ +#define NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_ + +// QT +#include +#include + +#include + +// ROS +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rviz_common/panel.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "nav2_msgs/action/dock_robot.hpp" +#include "nav2_msgs/action/undock_robot.hpp" + +class QLineEdit; +class QPushButton; + +namespace nav2_rviz_plugins +{ + +class DockingPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit DockingPanel(QWidget * parent = 0); + virtual ~DockingPanel(); + void onInitialize() override; + +private Q_SLOTS: + void onDockingButtonPressed(); + void onUndockingButtonPressed(); + void dockIdCheckbox(); + +private: + using Dock = nav2_msgs::action::DockRobot; + using Undock = nav2_msgs::action::UndockRobot; + 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; + + // Create label string from feedback msg + static inline QString getDockFeedbackLabel(Dock::Feedback msg = Dock::Feedback()); + + // Create label string from status msg + template + static inline std::string toLabel(T & msg); + + // Round off double to the specified precision and convert to string + static inline std::string toString(double val, int precision = 0); + + // Convert the dock state and error code to string + static inline std::string dockStateToString(int16_t state); + static inline std::string dockErrorToString(int16_t error_code); + + // The (non-spinning) client node used to invoke the action client + rclcpp::Node::SharedPtr client_node_; + // Timeout value when waiting for action servers to respond + std::chrono::milliseconds server_timeout_; + + // 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}; + QVBoxLayout * feedback_layout_{nullptr}; + QHBoxLayout * dock_id_layout_{nullptr}; + QHBoxLayout * dock_type_layout_{nullptr}; + QHBoxLayout * dock_pose_layout_{nullptr}; + QHBoxLayout * nav_stage_layout_{nullptr}; + + QComboBox * dock_type_{nullptr}; + QPushButton * docking_button_{nullptr}; + QPushButton * undocking_button_{nullptr}; + QCheckBox * use_dock_id_checkbox_{nullptr}; + QCheckBox * nav_to_staging_checkbox_{nullptr}; + + QLabel * docking_goal_status_indicator_{nullptr}; + QLabel * docking_feedback_indicator_{nullptr}; + QLabel * docking_result_indicator_{nullptr}; + + QLineEdit * dock_id_{nullptr}; + QLineEdit * dock_pose_x_{nullptr}; + QLineEdit * dock_pose_y_{nullptr}; + QLineEdit * dock_pose_yaw_{nullptr}; + + // The current state of the docking and undocking actions + bool docking_in_progress_ = false; + 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_; + + // 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_; +}; + +} // namespace nav2_rviz_plugins + +#endif // NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_ 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 759e242451..064c6c68ff 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -204,10 +204,6 @@ private Q_SLOTS: void resetUniqueId(); - // create label string from goal status msg - static inline QString getGoalStatusLabel( - int8_t status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN); - // create label string from feedback msg static inline QString getNavToPoseFeedbackLabel( nav2_msgs::action::NavigateToPose::Feedback msg = diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp index 0301b89399..1f6596e9cc 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp @@ -78,19 +78,6 @@ class Selector : public rviz_common::Panel void setSmoother(); void setProgressChecker(); - /* - * @brief Load the avaialble plugins into the combo box - * @param node The node to use for loading the plugins - * @param server_name The name of the server to load plugins for - * @param plugin_type The type of plugin to load - * @param combo_box The combo box to add the loaded plugins to - */ - void pluginLoader( - rclcpp::Node::SharedPtr node, - const std::string & server_name, - const std::string & plugin_type, - QComboBox * combo_box); - /* * @brief Set the selection from the combo box * @param combo_box The combo box to set the selection for diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp new file mode 100644 index 0000000000..1f286060f3 --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2024 Neobotix GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_RVIZ_PLUGINS__UTILS_HPP_ +#define NAV2_RVIZ_PLUGINS__UTILS_HPP_ + +#include + +#include + +#include "rclcpp/rclcpp.hpp" +#include "action_msgs/msg/goal_status.hpp" + +namespace nav2_rviz_plugins +{ + +/** + * @brief Load the avaialble plugins into the combo box + * @param node The node to use for loading the plugins + * @param server_failed if the server failed to load the plugins, false otherwise + * @param server_name The name of the server to load plugins for + * @param plugin_type The type of plugin to load + * @param combo_box The combo box to add the loaded plugins to + */ +void pluginLoader( + rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name, + const std::string & plugin_type, QComboBox * combo_box); + +// Create label string from goal status msg +QString getGoalStatusLabel( + std::string title = "Feedback", int8_t status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN); +} // namespace nav2_rviz_plugins + +#endif // NAV2_RVIZ_PLUGINS__UTILS_HPP_ diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 3b51f43dd4..90c4bdfa4d 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -28,7 +28,7 @@ urdf visualization_msgs yaml_cpp_vendor - + libqt5-core libqt5-gui libqt5-opengl diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index 5597636190..b392342638 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -18,6 +18,12 @@ The Nav2 rviz panel for selecting planners and controllers. + + The Nav2 rviz panel for dock and undock actions. + + diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp new file mode 100644 index 0000000000..909e9a74fd --- /dev/null +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -0,0 +1,486 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// C++ +#include + +// QT +#include +#include +#include +#include +#include + +#include "nav2_util/geometry_utils.hpp" +#include "nav2_rviz_plugins/docking_panel.hpp" +#include "nav2_rviz_plugins/utils.hpp" + +using namespace std::chrono_literals; + +namespace nav2_rviz_plugins +{ + +DockingPanel::DockingPanel(QWidget * parent) +: Panel(parent), + server_timeout_(100) +{ + client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); + + main_layout_ = new QVBoxLayout; + info_layout_ = new QHBoxLayout; + feedback_layout_ = new QVBoxLayout; + dock_id_layout_ = new QHBoxLayout; + dock_type_layout_ = new QHBoxLayout; + 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_goal_status_indicator_ = new QLabel; + docking_feedback_indicator_ = new QLabel; + docking_result_indicator_ = new QLabel; + use_dock_id_checkbox_ = new QCheckBox; + nav_to_staging_checkbox_ = new QCheckBox; + dock_id_ = new QLineEdit; + dock_pose_x_ = new QLineEdit; + 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); + 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); + + 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")); + 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_); + dock_pose_layout_->addWidget(new QLabel("Dock pose {X")); + dock_pose_layout_->addWidget(dock_pose_x_); + dock_pose_layout_->addWidget(new QLabel("Y")); + dock_pose_layout_->addWidget(dock_pose_y_); + 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")); + + 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(docking_button_); + main_layout_->addWidget(undocking_button_); + + setLayout(main_layout_); + timer_.start(200, this); + + dock_client_ = rclcpp_action::create_client(client_node_, "dock_robot"); + undock_client_ = rclcpp_action::create_client(client_node_, "undock_robot"); + + // 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); +} + +void DockingPanel::onInitialize() +{ + auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + // Create action feedback subscriber + docking_feedback_sub_ = node->create_subscription( + "dock_robot/_action/feedback", + 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 + docking_goal_status_sub_ = node->create_subscription( + "dock_robot/_action/status", + rclcpp::SystemDefaultsQoS(), + [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()); + } + }); + + undocking_goal_status_sub_ = node->create_subscription( + "undock_robot/_action/status", + rclcpp::SystemDefaultsQoS(), + [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; + } + }); +} + +DockingPanel::~DockingPanel() +{ +} + +void DockingPanel::onDockingButtonPressed() +{ + if (!docking_in_progress_) { + startDocking(); + } else { + cancelDocking(); + } +} + +void DockingPanel::onUndockingButtonPressed() +{ + if (!undocking_in_progress_) { + startUndocking(); + } else { + cancelUndocking(); + } +} + +void DockingPanel::startDocking() +{ + auto is_action_server_ready = + dock_client_->wait_for_action_server(std::chrono::seconds(5)); + if (!is_action_server_ready) { + RCLCPP_ERROR(client_node_->get_logger(), "dock_robot action server is not available."); + return; + } + + QComboBox * combo_box = dock_type_; + // If "default" option is selected, it gets removed and the next item is selected + if (combo_box->findText("Default") != -1) { + combo_box->removeItem(0); + } + + // If there are no plugins available, return + if (combo_box->count() == 0) { + return; + } + + // Send the goal to the action server + auto goal_msg = Dock::Goal(); + goal_msg.use_dock_id = use_dock_id_; + goal_msg.navigate_to_staging_pose = nav_to_staging_checkbox_->isChecked(); + if (use_dock_id_) { + if (dock_id_->text().isEmpty()) { + RCLCPP_ERROR(client_node_->get_logger(), "Dock id is empty."); + return; + } + goal_msg.dock_id = dock_id_->text().toStdString(); + + RCLCPP_INFO( + client_node_->get_logger(), "DockRobot will be called using dock id: %s", + goal_msg.dock_id.c_str()); + + } else { + if (dock_pose_x_->text().isEmpty() || dock_pose_y_->text().isEmpty() || + dock_pose_yaw_->text().isEmpty()) + { + RCLCPP_ERROR(client_node_->get_logger(), "Dock pose is empty."); + return; + } + goal_msg.dock_pose.header.frame_id = "map"; + goal_msg.dock_pose.header.stamp = client_node_->now(); + goal_msg.dock_pose.pose.position.x = dock_pose_x_->text().toDouble(); + goal_msg.dock_pose.pose.position.y = dock_pose_y_->text().toDouble(); + goal_msg.dock_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + dock_pose_yaw_->text().toDouble()); + goal_msg.dock_type = combo_box->currentText().toStdString(); + + RCLCPP_INFO( + client_node_->get_logger(), "DockRobot will be called using dock pose: (%f, %f) and type: %s", + goal_msg.dock_pose.pose.position.x, goal_msg.dock_pose.pose.position.y, + goal_msg.dock_type.c_str()); + } + + // Enable result awareness by providing an empty lambda function + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = [this](const DockGoalHandle::WrappedResult & result) { + dock_goal_handle_.reset(); + if (result.result->success) { + docking_result_indicator_->setText(""); + } else { + docking_result_indicator_->setText( + QString(std::string("(" + dockErrorToString(result.result->error_code) + ")").c_str())); + } + }; + + auto future_goal_handle = dock_client_->async_send_goal(goal_msg, send_goal_options); + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); + return; + } + + // Get the goal handle and save so that we can check on completion in the timer callback + dock_goal_handle_ = future_goal_handle.get(); + if (!dock_goal_handle_) { + RCLCPP_ERROR(client_node_->get_logger(), "Goal was rejected by server"); + return; + } + + timer_.start(200, this); +} + +void DockingPanel::startUndocking() +{ + auto is_action_server_ready = + undock_client_->wait_for_action_server(std::chrono::seconds(5)); + if (!is_action_server_ready) { + RCLCPP_ERROR(client_node_->get_logger(), "undock_robot action server is not available."); + return; + } + + QComboBox * combo_box = dock_type_; + // If "default" option is selected, it gets removed and the next item is selected + if (combo_box->findText("Default") != -1) { + combo_box->removeItem(0); + } + + // If there are no plugins available, return + if (combo_box->count() == 0) { + return; + } + + // Send the goal to the action server + auto goal_msg = Undock::Goal(); + goal_msg.dock_type = combo_box->currentText().toStdString(); + + RCLCPP_INFO( + client_node_->get_logger(), "UndockRobot will be called using dock type: %s", + goal_msg.dock_type.c_str()); + + // Enable result awareness by providing an empty lambda function + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = [this](const UndockGoalHandle::WrappedResult & result) { + undock_goal_handle_.reset(); + if (result.result->success) { + docking_result_indicator_->setText(""); + } else { + docking_result_indicator_->setText( + QString(std::string("(" + dockErrorToString(result.result->error_code) + ")").c_str())); + } + }; + + auto future_goal_handle = undock_client_->async_send_goal(goal_msg, send_goal_options); + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); + return; + } + + // Get the goal handle and save so that we can check on completion in the timer callback + undock_goal_handle_ = future_goal_handle.get(); + if (!undock_goal_handle_) { + RCLCPP_ERROR(client_node_->get_logger(), "Goal was rejected by server"); + return; + } + + timer_.start(200, this); +} + +void DockingPanel::dockIdCheckbox() +{ + if (use_dock_id_checkbox_->isChecked()) { + use_dock_id_ = true; + dock_id_->setEnabled(true); + dock_pose_x_->setEnabled(false); + dock_pose_y_->setEnabled(false); + dock_pose_yaw_->setEnabled(false); + } else { + use_dock_id_ = false; + dock_id_->setEnabled(false); + dock_pose_x_->setEnabled(true); + dock_pose_y_->setEnabled(true); + dock_pose_yaw_->setEnabled(true); + } +} + +void DockingPanel::cancelDocking() +{ + if (dock_goal_handle_) { + auto future_cancel = dock_client_->async_cancel_goal(dock_goal_handle_); + + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); + } else { + dock_goal_handle_.reset(); + } + } +} + +void DockingPanel::cancelUndocking() +{ + if (undock_goal_handle_) { + auto future_cancel = undock_client_->async_cancel_goal(undock_goal_handle_); + + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); + } else { + undock_goal_handle_.reset(); + } + } +} + +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); + } + + // 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; + } + + timer_.stop(); + } +} + +inline QString DockingPanel::getDockFeedbackLabel(Dock::Feedback msg) +{ + return QString(std::string("" + toLabel(msg) + "
").c_str()); +} + +template +inline std::string DockingPanel::toLabel(T & msg) +{ + return std::string( + "State:" + + dockStateToString(msg.state) + + "Time taken:" + + toString(rclcpp::Duration(msg.docking_time).seconds(), 0) + " s" + "Retries:" + + std::to_string(msg.num_retries) + + ""); +} + +inline std::string DockingPanel::toString(double val, int precision) +{ + std::ostringstream out; + out.precision(precision); + out << std::fixed << val; + return out.str(); +} + +inline std::string DockingPanel::dockStateToString(int16_t state) +{ + switch (state) { + case 0: + return "none"; + case 1: + return "nav. to staging pose"; + case 2: + return "initial perception"; + case 3: + return "controlling"; + case 4: + return "wait for charge"; + case 5: + return "retry"; + default: + return "none"; + } +} + +inline std::string DockingPanel::dockErrorToString(int16_t error_code) +{ + switch (error_code) { + case 0: + return "none"; + case 901: + return "dock not in database"; + case 902: + return "dock not valid"; + case 903: + return "failed to stage"; + case 904: + return "failed to detect dock"; + case 905: + return "failed to control"; + case 906: + return "failed to charge"; + case 999: + default: + return "unknown"; + } +} + +} // namespace nav2_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::DockingPanel, rviz_common::Panel) diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index f7ce1d8220..64ba0a329b 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -28,6 +28,7 @@ #include #include "nav2_rviz_plugins/goal_common.hpp" +#include "nav2_rviz_plugins/utils.hpp" #include "rviz_common/display_context.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" #include "yaml-cpp/yaml.h" @@ -75,22 +76,22 @@ Nav2Panel::Nav2Panel(QWidget * parent) const char * nft_goal_msg = "Start navigating through poses"; const char * cancel_waypoint_msg = "Cancel waypoint / viapoint accumulation mode"; - const QString navigation_active("" + const QString navigation_active("
Navigation:
" "
Navigation:active
"); - const QString navigation_inactive("" + const QString navigation_inactive("
Navigation:
" "
Navigation:inactive
"); - const QString navigation_unknown("" + const QString navigation_unknown("
Navigation:
" "
Navigation:unknown
"); - const QString localization_active("" + const QString localization_active("
Localization:
" "
Localization:active
"); - const QString localization_inactive("" + const QString localization_inactive("
Localization:
" "
Localization:inactive
"); - const QString localization_unknown("" + const QString localization_unknown("
Localization:
" "
Localization:unknown
"); navigation_status_indicator_->setText(navigation_unknown); localization_status_indicator_->setText(localization_unknown); - navigation_goal_status_indicator_->setText(getGoalStatusLabel()); + navigation_goal_status_indicator_->setText(nav2_rviz_plugins::getGoalStatusLabel()); number_of_loops_->setText("Num of loops"); navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel()); navigation_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); @@ -481,7 +482,7 @@ Nav2Panel::Nav2Panel(QWidget * parent) initial_thread_, &InitialThread::navigationInactive, [this, navigation_inactive] { navigation_status_indicator_->setText(navigation_inactive); - navigation_goal_status_indicator_->setText(getGoalStatusLabel()); + navigation_goal_status_indicator_->setText(nav2_rviz_plugins::getGoalStatusLabel()); navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel()); }); QObject::connect( @@ -821,7 +822,7 @@ Nav2Panel::onInitialize() rclcpp::SystemDefaultsQoS(), [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { navigation_goal_status_indicator_->setText( - getGoalStatusLabel(msg->status_list.back().status)); + nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); // Clearing all the stored values once reaching the final goal if ( loop_count_ == stoi(nr_of_loops_->displayText().toStdString()) && @@ -840,7 +841,7 @@ Nav2Panel::onInitialize() rclcpp::SystemDefaultsQoS(), [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { navigation_goal_status_indicator_->setText( - getGoalStatusLabel(msg->status_list.back().status)); + nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel()); } @@ -1440,41 +1441,6 @@ Nav2Panel::updateWpNavigationMarkers() wp_navigation_markers_pub_->publish(std::move(marker_array)); } -inline QString -Nav2Panel::getGoalStatusLabel(int8_t status) -{ - std::string status_str; - switch (status) { - case action_msgs::msg::GoalStatus::STATUS_EXECUTING: - status_str = "active"; - break; - - case action_msgs::msg::GoalStatus::STATUS_SUCCEEDED: - status_str = "reached"; - break; - - case action_msgs::msg::GoalStatus::STATUS_CANCELED: - status_str = "canceled"; - break; - - case action_msgs::msg::GoalStatus::STATUS_ABORTED: - status_str = "aborted"; - break; - - case action_msgs::msg::GoalStatus::STATUS_UNKNOWN: - status_str = "unknown"; - break; - - default: - status_str = "inactive"; - break; - } - return QString( - std::string( - "
Feedback:" + - status_str + "
").c_str()); -} - inline QString Nav2Panel::getNavToPoseFeedbackLabel(nav2_msgs::action::NavigateToPose::Feedback msg) { diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index 1df075f82d..102e123e27 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "nav2_rviz_plugins/selector.hpp" +#include "nav2_rviz_plugins/utils.hpp" #include "rviz_common/display_context.hpp" using namespace std::chrono_literals; @@ -146,62 +147,21 @@ void Selector::setProgressChecker() setSelection(progress_checker_, pub_progress_checker_); } -// Load the available plugins into the combo box -void Selector::pluginLoader( - rclcpp::Node::SharedPtr node, - const std::string & server_name, - const std::string & plugin_type, - QComboBox * combo_box) -{ - auto parameter_client = std::make_shared(node, server_name); - - // Do not load the plugins if the combo box is already populated - if (combo_box->count() > 0) { - return; - } - - // Wait for the service to be available before calling it - bool server_unavailable = false; - while (!parameter_client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - 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()); - server_unavailable = true; - server_failed_ = true; - break; - } - - // Loading the plugins into the combo box - if (!plugins_loaded_) { - // If server unavaialble, let the combo box be empty - if (server_unavailable) { - return; - } - combo_box->addItem("Default"); - auto parameters = parameter_client->get_parameters({plugin_type}); - auto str_arr = parameters[0].as_string_array(); - for (auto str : str_arr) { - combo_box->addItem(QString::fromStdString(str)); - } - combo_box->setCurrentText("Default"); - } -} - void Selector::timerEvent(QTimerEvent * event) { if (event->timerId() == timer_.timerId()) { if (!plugins_loaded_) { - pluginLoader(client_node_, "controller_server", "controller_plugins", controller_); - pluginLoader(client_node_, "planner_server", "planner_plugins", planner_); - pluginLoader(client_node_, "controller_server", "goal_checker_plugins", goal_checker_); - pluginLoader(client_node_, "smoother_server", "smoother_plugins", smoother_); - pluginLoader( - client_node_, "controller_server", "progress_checker_plugins", + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "controller_plugins", controller_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "planner_server", "planner_plugins", planner_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "goal_checker_plugins", goal_checker_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "smoother_server", "smoother_plugins", smoother_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "progress_checker_plugins", progress_checker_); plugins_loaded_ = true; diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp new file mode 100644 index 0000000000..6c7fcbbad0 --- /dev/null +++ b/nav2_rviz_plugins/src/utils.cpp @@ -0,0 +1,97 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2024 Neobotix GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "nav2_rviz_plugins/utils.hpp" + +namespace nav2_rviz_plugins +{ + +void pluginLoader( + rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name, + const std::string & plugin_type, QComboBox * combo_box) +{ + auto parameter_client = std::make_shared(node, server_name); + + // Do not load the plugins if the combo box is already populated + if (combo_box->count() > 0) { + return; + } + + // Wait for the service to be available before calling it + bool server_unavailable = false; + while (!parameter_client->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + 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()); + server_unavailable = true; + server_failed = true; + break; + } + + // Loading the plugins into the combo box + // If server unavaialble, let the combo box be empty + if (server_unavailable) { + return; + } + combo_box->addItem("Default"); + auto parameters = parameter_client->get_parameters({plugin_type}); + auto str_arr = parameters[0].as_string_array(); + for (auto str : str_arr) { + combo_box->addItem(QString::fromStdString(str)); + } + combo_box->setCurrentText("Default"); +} + +QString getGoalStatusLabel(std::string title, int8_t status) +{ + std::string status_str; + switch (status) { + case action_msgs::msg::GoalStatus::STATUS_EXECUTING: + status_str = "active"; + break; + + case action_msgs::msg::GoalStatus::STATUS_SUCCEEDED: + status_str = "reached"; + break; + + case action_msgs::msg::GoalStatus::STATUS_CANCELED: + status_str = "canceled"; + break; + + case action_msgs::msg::GoalStatus::STATUS_ABORTED: + status_str = "aborted"; + break; + + case action_msgs::msg::GoalStatus::STATUS_UNKNOWN: + status_str = "unknown"; + break; + + default: + status_str = "inactive"; + break; + } + return QString( + std::string( + "
" + title + ":" + + status_str + "
").c_str()); +} + +} // namespace nav2_rviz_plugins