Skip to content

Commit 8861e9d

Browse files
ajtudelastevedanomodolor
ajtudela
authored and
stevedanomodolor
committed
Dock panel (ros-navigation#4458)
* Initial docking panel Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Only one goal status Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added dock pose Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix size of text Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update rviz Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update rviz config Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>
1 parent aee360e commit 8861e9d

File tree

13 files changed

+836
-122
lines changed

13 files changed

+836
-122
lines changed

‎nav2_bringup/rviz/nav2_default_view.rviz‎

Lines changed: 14 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ Panels:
88
- /TF1/Frames1
99
- /TF1/Tree1
1010
Splitter Ratio: 0.5833333134651184
11-
Tree Height: 606
11+
Tree Height: 406
1212
- Class: rviz_common/Selection
1313
Name: Selection
1414
- Class: rviz_common/Tool Properties
@@ -25,6 +25,8 @@ Panels:
2525
Name: Navigation 2
2626
- Class: nav2_rviz_plugins/Selector
2727
Name: Selector
28+
- Class: nav2_rviz_plugins/Docking
29+
Name: Docking
2830
Visualization Manager:
2931
Class: ""
3032
Displays:
@@ -581,20 +583,24 @@ Visualization Manager:
581583
Window Geometry:
582584
Displays:
583585
collapsed: false
584-
Height: 932
586+
Docking:
587+
collapsed: false
588+
Height: 1043
585589
Hide Left Dock: false
586-
Hide Right Dock: true
590+
Hide Right Dock: false
587591
Navigation 2:
588592
collapsed: false
589-
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002de000000a90000008100fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
593+
QMainWindow State: 000000ff00000000fd00000004000000000000016a000003b9fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006900730070006c006100790073010000003d000001d3000000c900fffffffb00000018004e0061007600690067006100740069006f006e002000320100000216000001e0000001e000fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000015b000003b9fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000001b5000000a400fffffffb0000000e0044006f0063006b0069006e006701000001f8000001320000013200fffffffb0000001000530065006c006500630074006f00720100000330000000c6000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000469000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
590594
RealsenseCamera:
591595
collapsed: false
592596
Selection:
593597
collapsed: false
598+
Selector:
599+
collapsed: false
594600
Tool Properties:
595601
collapsed: false
596602
Views:
597-
collapsed: true
598-
Width: 1545
599-
X: 696
600-
Y: 229
603+
collapsed: false
604+
Width: 1850
605+
X: 1990
606+
Y: 0

‎nav2_rviz_plugins/CMakeLists.txt‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,11 +34,13 @@ find_package(visualization_msgs REQUIRED)
3434
find_package(yaml_cpp_vendor REQUIRED)
3535

3636
set(nav2_rviz_plugins_headers_to_moc
37+
include/nav2_rviz_plugins/docking_panel.hpp
3738
include/nav2_rviz_plugins/goal_pose_updater.hpp
3839
include/nav2_rviz_plugins/goal_common.hpp
3940
include/nav2_rviz_plugins/goal_tool.hpp
4041
include/nav2_rviz_plugins/nav2_panel.hpp
4142
include/nav2_rviz_plugins/selector.hpp
43+
include/nav2_rviz_plugins/utils.hpp
4244
include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp
4345
include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp
4446
)
@@ -50,9 +52,11 @@ include_directories(
5052
set(library_name ${PROJECT_NAME})
5153

5254
add_library(${library_name} SHARED
55+
src/docking_panel.cpp
5356
src/goal_tool.cpp
5457
src/nav2_panel.cpp
5558
src/selector.cpp
59+
src/utils.cpp
5660
src/particle_cloud_display/flat_weighted_arrows_array.cpp
5761
src/particle_cloud_display/particle_cloud_display.cpp
5862
${nav2_rviz_plugins_headers_to_moc}
@@ -111,6 +115,11 @@ install(
111115
DESTINATION include/
112116
)
113117

118+
install(
119+
DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons"
120+
DESTINATION "share/${PROJECT_NAME}"
121+
)
122+
114123
if(BUILD_TESTING)
115124
find_package(ament_lint_auto REQUIRED)
116125
ament_lint_auto_find_test_dependencies()

‎nav2_rviz_plugins/icons/classes/Docking.svg‎

Lines changed: 18 additions & 0 deletions
Loading

‎nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp‎

Lines changed: 137 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
// Copyright (c) 2024 Alberto J. Tudela Roldán
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_
16+
#define NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_
17+
18+
// QT
19+
#include <QtWidgets>
20+
#include <QBasicTimer>
21+
22+
#include <string>
23+
24+
// ROS
25+
#include "rclcpp/rclcpp.hpp"
26+
#include "rclcpp_action/rclcpp_action.hpp"
27+
#include "rviz_common/panel.hpp"
28+
#include "sensor_msgs/msg/battery_state.hpp"
29+
#include "nav2_msgs/action/dock_robot.hpp"
30+
#include "nav2_msgs/action/undock_robot.hpp"
31+
32+
class QLineEdit;
33+
class QPushButton;
34+
35+
namespace nav2_rviz_plugins
36+
{
37+
38+
class DockingPanel : public rviz_common::Panel
39+
{
40+
Q_OBJECT
41+
42+
public:
43+
explicit DockingPanel(QWidget * parent = 0);
44+
virtual ~DockingPanel();
45+
void onInitialize() override;
46+
47+
private Q_SLOTS:
48+
void onDockingButtonPressed();
49+
void onUndockingButtonPressed();
50+
void dockIdCheckbox();
51+
52+
private:
53+
using Dock = nav2_msgs::action::DockRobot;
54+
using Undock = nav2_msgs::action::UndockRobot;
55+
using DockGoalHandle = rclcpp_action::ClientGoalHandle<Dock>;
56+
using UndockGoalHandle = rclcpp_action::ClientGoalHandle<Undock>;
57+
58+
// Start the actions
59+
void startDocking();
60+
void startUndocking();
61+
62+
// Cancel the actions
63+
void cancelDocking();
64+
void cancelUndocking();
65+
66+
// The (non-spinning) client node used to invoke the action client
67+
void timerEvent(QTimerEvent * event) override;
68+
69+
// Create label string from feedback msg
70+
static inline QString getDockFeedbackLabel(Dock::Feedback msg = Dock::Feedback());
71+
72+
// Create label string from status msg
73+
template<typename T>
74+
static inline std::string toLabel(T & msg);
75+
76+
// Round off double to the specified precision and convert to string
77+
static inline std::string toString(double val, int precision = 0);
78+
79+
// Convert the dock state and error code to string
80+
static inline std::string dockStateToString(int16_t state);
81+
static inline std::string dockErrorToString(int16_t error_code);
82+
83+
// The (non-spinning) client node used to invoke the action client
84+
rclcpp::Node::SharedPtr client_node_;
85+
// Timeout value when waiting for action servers to respond
86+
std::chrono::milliseconds server_timeout_;
87+
88+
// Flags to indicate if the plugins have been loaded
89+
bool plugins_loaded_ = false;
90+
bool server_failed_ = false;
91+
bool tried_once_ = false;
92+
QBasicTimer timer_;
93+
94+
QVBoxLayout * main_layout_{nullptr};
95+
QHBoxLayout * info_layout_{nullptr};
96+
QVBoxLayout * feedback_layout_{nullptr};
97+
QHBoxLayout * dock_id_layout_{nullptr};
98+
QHBoxLayout * dock_type_layout_{nullptr};
99+
QHBoxLayout * dock_pose_layout_{nullptr};
100+
QHBoxLayout * nav_stage_layout_{nullptr};
101+
102+
QComboBox * dock_type_{nullptr};
103+
QPushButton * docking_button_{nullptr};
104+
QPushButton * undocking_button_{nullptr};
105+
QCheckBox * use_dock_id_checkbox_{nullptr};
106+
QCheckBox * nav_to_staging_checkbox_{nullptr};
107+
108+
QLabel * docking_goal_status_indicator_{nullptr};
109+
QLabel * docking_feedback_indicator_{nullptr};
110+
QLabel * docking_result_indicator_{nullptr};
111+
112+
QLineEdit * dock_id_{nullptr};
113+
QLineEdit * dock_pose_x_{nullptr};
114+
QLineEdit * dock_pose_y_{nullptr};
115+
QLineEdit * dock_pose_yaw_{nullptr};
116+
117+
// The current state of the docking and undocking actions
118+
bool docking_in_progress_ = false;
119+
bool undocking_in_progress_ = false;
120+
bool use_dock_id_ = false;
121+
122+
// The Dock and Undock action client
123+
rclcpp_action::Client<Dock>::SharedPtr dock_client_;
124+
rclcpp_action::Client<Undock>::SharedPtr undock_client_;
125+
DockGoalHandle::SharedPtr dock_goal_handle_;
126+
UndockGoalHandle::SharedPtr undock_goal_handle_;
127+
128+
// Docking / Undocking action feedback subscribers
129+
rclcpp::Subscription<Dock::Impl::FeedbackMessage>::SharedPtr docking_feedback_sub_;
130+
rclcpp::Subscription<Undock::Impl::FeedbackMessage>::SharedPtr undocking_feedback_sub_;
131+
rclcpp::Subscription<Dock::Impl::GoalStatusMessage>::SharedPtr docking_goal_status_sub_;
132+
rclcpp::Subscription<Undock::Impl::GoalStatusMessage>::SharedPtr undocking_goal_status_sub_;
133+
};
134+
135+
} // namespace nav2_rviz_plugins
136+
137+
#endif // NAV2_RVIZ_PLUGINS__DOCKING_PANEL_HPP_

‎nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -204,10 +204,6 @@ private Q_SLOTS:
204204

205205
void resetUniqueId();
206206

207-
// create label string from goal status msg
208-
static inline QString getGoalStatusLabel(
209-
int8_t status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
210-
211207
// create label string from feedback msg
212208
static inline QString getNavToPoseFeedbackLabel(
213209
nav2_msgs::action::NavigateToPose::Feedback msg =

‎nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp‎

Lines changed: 0 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -78,19 +78,6 @@ class Selector : public rviz_common::Panel
7878
void setSmoother();
7979
void setProgressChecker();
8080

81-
/*
82-
* @brief Load the avaialble plugins into the combo box
83-
* @param node The node to use for loading the plugins
84-
* @param server_name The name of the server to load plugins for
85-
* @param plugin_type The type of plugin to load
86-
* @param combo_box The combo box to add the loaded plugins to
87-
*/
88-
void pluginLoader(
89-
rclcpp::Node::SharedPtr node,
90-
const std::string & server_name,
91-
const std::string & plugin_type,
92-
QComboBox * combo_box);
93-
9481
/*
9582
* @brief Set the selection from the combo box
9683
* @param combo_box The combo box to set the selection for

‎nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
// Copyright (c) 2019 Intel Corporation
2+
// Copyright (c) 2024 Neobotix GmbH
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#ifndef NAV2_RVIZ_PLUGINS__UTILS_HPP_
17+
#define NAV2_RVIZ_PLUGINS__UTILS_HPP_
18+
19+
#include <QtWidgets>
20+
21+
#include <string>
22+
23+
#include "rclcpp/rclcpp.hpp"
24+
#include "action_msgs/msg/goal_status.hpp"
25+
26+
namespace nav2_rviz_plugins
27+
{
28+
29+
/**
30+
* @brief Load the avaialble plugins into the combo box
31+
* @param node The node to use for loading the plugins
32+
* @param server_failed if the server failed to load the plugins, false otherwise
33+
* @param server_name The name of the server to load plugins for
34+
* @param plugin_type The type of plugin to load
35+
* @param combo_box The combo box to add the loaded plugins to
36+
*/
37+
void pluginLoader(
38+
rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name,
39+
const std::string & plugin_type, QComboBox * combo_box);
40+
41+
// Create label string from goal status msg
42+
QString getGoalStatusLabel(
43+
std::string title = "Feedback", int8_t status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
44+
} // namespace nav2_rviz_plugins
45+
46+
#endif // NAV2_RVIZ_PLUGINS__UTILS_HPP_

‎nav2_rviz_plugins/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828
<depend>urdf</depend>
2929
<depend>visualization_msgs</depend>
3030
<depend>yaml_cpp_vendor</depend>
31-
31+
3232
<exec_depend>libqt5-core</exec_depend>
3333
<exec_depend>libqt5-gui</exec_depend>
3434
<exec_depend>libqt5-opengl</exec_depend>

‎nav2_rviz_plugins/plugins_description.xml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,12 @@
1818
<description>The Nav2 rviz panel for selecting planners and controllers.</description>
1919
</class>
2020

21+
<class name="nav2_rviz_plugins/Docking"
22+
type="nav2_rviz_plugins::DockingPanel"
23+
base_class_type="rviz_common::Panel">
24+
<description>The Nav2 rviz panel for dock and undock actions.</description>
25+
</class>
26+
2127
<class name="nav2_rviz_plugins/ParticleCloud"
2228
type="nav2_rviz_plugins::ParticleCloudDisplay"
2329
base_class_type="rviz_common::Display">

0 commit comments

Comments
 (0)