-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Initial docking panel Signed-off-by: Alberto Tudela <[email protected]> * Only one goal status Signed-off-by: Alberto Tudela <[email protected]> * Added dock pose Signed-off-by: Alberto Tudela <[email protected]> * Fix size of text Signed-off-by: Alberto Tudela <[email protected]> * Update rviz Signed-off-by: Alberto Tudela <[email protected]> * Update rviz config Signed-off-by: Alberto Tudela <[email protected]> --------- Signed-off-by: Alberto Tudela <[email protected]>
- Loading branch information
Showing
13 changed files
with
836 additions
and
122 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
137 changes: 137 additions & 0 deletions
137
nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.