diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp index cc979bf1c32..8271e1d466a 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp @@ -1,19 +1,33 @@ -#ifndef SELECTOR_HPP_ -#define SELECTOR_HPP_ +// 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__SELECTOR_HPP_ +#define NAV2_RVIZ_PLUGINS__SELECTOR_HPP_ -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/panel.hpp" #include -#include "vector" -#include "memory" -#include "string" #include #include #include -#include #include #include #include + +#include "rclcpp/rclcpp.hpp" +#include "rviz_common/panel.hpp" +#include "vector" +#include "memory" +#include "string" #include "std_msgs/msg/string.hpp" class QPushButton; @@ -22,49 +36,49 @@ namespace nav2_rviz_plugins { class Selector : public rviz_common::Panel { - Q_OBJECT + Q_OBJECT public: explicit Selector(QWidget * parent = 0); ~Selector(); - void onInitialize() override; private: - void timerEvent(QTimerEvent * event) override; - // The (non-spinning) client node used to invoke the action client + // The (non-spinning) client node used to invoke the action client + void timerEvent(QTimerEvent * event) override; + rclcpp::Node::SharedPtr client_node_; - rclcpp::Node::SharedPtr node_; rclcpp::Publisher::SharedPtr pub_controller_; - std::string controller_name_; + rclcpp::Publisher::SharedPtr pub_planner_; + rclcpp::TimerBase::SharedPtr rclcpp_timer_; bool plugins_loaded_ = false; QBasicTimer timer_; - QImage * image_; - QLabel * imgDisplayLabel; QVBoxLayout * main_layout; - QVBoxLayout * left_layout; - QVBoxLayout * right_layout; - QComboBox * controller; - QComboBox * planner; - QComboBox * controller_params; - QComboBox * planner_params; + QComboBox * controller_; + QComboBox * planner_; - QToolButton toggleButton; - QScrollArea contentArea; - QParallelAnimationGroup toggleAnimation; - int animationDuration{300}; - rclcpp::TimerBase::SharedPtr rclcpp_timer_; - void timer_callback(); void start_ros_timer(); void setController(); + void setPlanner(); + + /* + * @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); protected: - QVBoxLayout * layout1 = new QVBoxLayout; -// QComboBox * combo = new QComboBox; + QVBoxLayout * layout1 = new QVBoxLayout; }; -} // namespace nav2_rviz_plugins - +} // namespace nav2_rviz_plugins -#endif // SELECTOR_HPP_ \ No newline at end of file +#endif // NAV2_RVIZ_PLUGINS__SELECTOR_HPP_ diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index cd383fc0869..eb6950ea4a1 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -1,6 +1,19 @@ +// 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 "nav2_rviz_plugins/selector.hpp" #include "rviz_common/display_context.hpp" -#include using namespace std::chrono_literals; @@ -9,136 +22,100 @@ namespace nav2_rviz_plugins Selector::Selector(QWidget * parent) : Panel(parent) { - client_node_ = std::make_shared("example"); + client_node_ = std::make_shared("nav2_rviz_selector_node"); rclcpp::QoS qos(rclcpp::KeepLast(1)); qos.transient_local().reliable(); - pub_controller_ = client_node_->create_publisher("controller_selector", qos); + pub_controller_ = + client_node_->create_publisher("controller_selector", qos); + pub_planner_ = client_node_->create_publisher("planner_selector", qos); main_layout = new QVBoxLayout; - left_layout = new QVBoxLayout; - right_layout = new QVBoxLayout; - controller = new QComboBox; - controller_params = new QComboBox; - planner = new QComboBox; - planner_params = new QComboBox; - - image_ = new QImage("nav2_logo.png"); - imgDisplayLabel = new QLabel(""); - imgDisplayLabel->setPixmap(QPixmap::fromImage(*image_)); - imgDisplayLabel->adjustSize(); + controller_ = new QComboBox; + planner_ = new QComboBox; main_layout->setContentsMargins(10, 10, 10, 10); - left_layout->addWidget(imgDisplayLabel); - left_layout->setContentsMargins(5, 5, 5, 5); - right_layout->addWidget(new QLabel("Controller")); - right_layout->addWidget(controller); - right_layout->addWidget(new QLabel("Planner")); - right_layout->addWidget(planner); - right_layout->addWidget(new QLabel("Controller params")); - right_layout->addWidget(controller_params); - right_layout->addWidget(new QLabel("Planner params")); - right_layout->addWidget(planner_params); - - main_layout->addLayout(left_layout); - main_layout->addLayout(right_layout); + + main_layout->addWidget(new QLabel("Controller")); + main_layout->addWidget(controller_); + main_layout->addWidget(new QLabel("Planner")); + main_layout->addWidget(planner_); setLayout(main_layout); timer_.start(200, this); connect( - controller, QOverload::of(&QComboBox::activated), this, + controller_, QOverload::of(&QComboBox::activated), this, &Selector::setController); + connect( + planner_, QOverload::of(&QComboBox::activated), this, + &Selector::setPlanner); } Selector::~Selector() { } +// Publish the selected controller void Selector::setController() { std_msgs::msg::String msg; - controller_name_ = controller->currentText().toStdString(); - msg.data = controller_name_; + msg.data = controller_->currentText().toStdString(); pub_controller_->publish(msg); timer_.start(200, this); } -void -Selector::onInitialize() +// Publish the selected planner +void Selector::setPlanner() { - auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + std_msgs::msg::String msg; + msg.data = planner_->currentText().toStdString(); + + pub_planner_->publish(msg); + timer_.start(200, this); } -void -Selector::timerEvent(QTimerEvent * event) +// 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) { - if (event->timerId() == timer_.timerId()) { - auto parameter_client_con = std::make_shared(client_node_, "controller_server"); - QStringList id_list; - - while (!parameter_client_con->wait_for_service(1s)) { + auto parameter_client = std::make_shared(node, server_name); + + // Wait for the service to be available before calling it + while (!parameter_client->wait_for_service(1s)) { if (!rclcpp::ok()) { - RCLCPP_ERROR(client_node_->get_logger(), "Interrupted while waiting for the service. Exiting."); + RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); rclcpp::shutdown(); } - RCLCPP_INFO(client_node_->get_logger(), "service not available, waiting again..."); - } - - if (!plugins_loaded_) { - auto parameters_con = parameter_client_con->get_parameters({"controller_plugins"}); - auto str_arr = parameters_con[0].as_string_array(); - for (auto str: str_arr) - id_list.push_back(QString::fromStdString(str)); - controller->addItems(id_list); - id_list.clear(); - } else { - auto parameters_con = parameter_client_con->get_parameters({"FollowPath.plugin"}); - std::cout<addItems(id_list); - // id_list.clear(); - } - const auto parameters = parameter_client_con->list_parameters({}, 50); // Timeout in seconds + RCLCPP_INFO(node->get_logger(), "service not available, waiting again..."); + } - for (const auto ¶m_name : parameters.names) { - std::cout << param_name << std::endl; - } - auto parameter_client = std::make_shared(client_node_, "planner_server"); - - while (!parameter_client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(client_node_->get_logger(), "Interrupted while waiting for the service. Exiting."); - rclcpp::shutdown(); - } - RCLCPP_INFO(client_node_->get_logger(), "service not available, waiting again..."); + // Loading the plugins into the combo box + if (!plugins_loaded_) { + 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)); } + } +} +void +Selector::timerEvent(QTimerEvent * event) +{ + if (event->timerId() == timer_.timerId()) { if (!plugins_loaded_) { - auto parameters = parameter_client->get_parameters({"planner_plugins"}); - auto str_arr1 = parameters[0].as_string_array(); - for (auto str: str_arr1) - id_list.push_back(QString::fromStdString(str)); - planner->addItems(id_list); - id_list.clear(); + pluginLoader(client_node_, "controller_server", "controller_plugins", controller_); + pluginLoader(client_node_, "planner_server", "planner_plugins", planner_); plugins_loaded_ = true; - } else { - auto parameters_con = parameter_client_con->get_parameters({controller_name_}); - // auto str_arr = parameters_con.as_string_array(); - // for (auto str: str_arr) - // id_list.push_back(QString::fromStdString(str)); - // controller_params->addItems(id_list); - // id_list.clear(); } - timer_.stop(); } - // if (controller->count() != 0 && planner->count() != 0) - // start_ros_timer(); } void @@ -147,7 +124,6 @@ Selector::start_ros_timer() rclcpp::spin(node_); } -} // namespace nav2_rviz_plugins - +} // namespace nav2_rviz_plugins #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::Selector, rviz_common::Panel) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::Selector, rviz_common::Panel)