+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 f7ce1d82209..64ba0a329b6 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("| Navigation: | "
+ const QString navigation_active("");
- const QString navigation_inactive("| Navigation: | "
+ const QString navigation_inactive("");
- const QString navigation_unknown("| Navigation: | "
+ const QString navigation_unknown("");
- const QString localization_active("| Localization: | "
+ const QString localization_active("");
- const QString localization_inactive("| Localization: | "
+ const QString localization_inactive("");
- const QString localization_unknown("| Localization: | "
+ const QString 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 1df075f82d3..102e123e271 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 00000000000..6c7fcbbad03
--- /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