Dock panel

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

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()

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 <QtWidgets>
+#include <QBasicTimer>
+
+#include <string>
+
+// 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<Dock>;
+  using UndockGoalHandle = rclcpp_action::ClientGoalHandle<Undock>;
+
+  // 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<typename T>
+  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<Dock>::SharedPtr dock_client_;
+  rclcpp_action::Client<Undock>::SharedPtr undock_client_;
+  DockGoalHandle::SharedPtr dock_goal_handle_;
+  UndockGoalHandle::SharedPtr undock_goal_handle_;
+
+  // Docking / Undocking action feedback subscribers
+  rclcpp::Subscription<Dock::Impl::FeedbackMessage>::SharedPtr docking_feedback_sub_;
+  rclcpp::Subscription<Undock::Impl::FeedbackMessage>::SharedPtr undocking_feedback_sub_;
+  rclcpp::Subscription<Dock::Impl::GoalStatusMessage>::SharedPtr docking_goal_status_sub_;
+  rclcpp::Subscription<Undock::Impl::GoalStatusMessage>::SharedPtr undocking_goal_status_sub_;
+};
+
+}  // namespace nav2_rviz_plugins
+
+#endif  // NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_

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 =

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

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 <QtWidgets>
+
+#include <string>
+
+#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_

nav2_rviz_plugins/package.xml

@@ -28,7 +28,7 @@
   <depend>urdf</depend>
   <depend>visualization_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
-  
+
   <exec_depend>libqt5-core</exec_depend>
   <exec_depend>libqt5-gui</exec_depend>
   <exec_depend>libqt5-opengl</exec_depend>

nav2_rviz_plugins/plugins_description.xml

@@ -18,6 +18,12 @@
     <description>The Nav2 rviz panel for selecting planners and controllers.</description>
   </class>
 
+  <class name="nav2_rviz_plugins/Docking"
+         type="nav2_rviz_plugins::DockingPanel"
+         base_class_type="rviz_common::Panel">
+    <description>The Nav2 rviz panel for dock and undock actions.</description>
+  </class>
+
   <class name="nav2_rviz_plugins/ParticleCloud"
          type="nav2_rviz_plugins::ParticleCloudDisplay"
          base_class_type="rviz_common::Display">