selector.cpp

// 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 "nav2_rviz_plugins/utils.hpp"
#include "rviz_common/display_context.hpp"
#include "rclcpp/rate.hpp"
 
using namespace std::chrono_literals;
 
namespace nav2_rviz_plugins
{
Selector::Selector(QWidget * parent)
: Panel(parent)
{
  client_node_ = std::make_shared<rclcpp::Node>("nav2_rviz_selector_node");  //  nosemgrep
  rclcpp::QoS qos(rclcpp::KeepLast(1));
  qos.transient_local().reliable();
 
  pub_controller_ =
    client_node_->create_publisher<std_msgs::msg::String>("controller_selector", qos);
  pub_planner_ = client_node_->create_publisher<std_msgs::msg::String>("planner_selector", qos);
  pub_goal_checker_ =
    client_node_->create_publisher<std_msgs::msg::String>("goal_checker_selector", qos);
  pub_smoother_ = client_node_->create_publisher<std_msgs::msg::String>("smoother_selector", qos);
  pub_progress_checker_ =
    client_node_->create_publisher<std_msgs::msg::String>("progress_checker_selector", qos);
  pub_path_handler_ =
    client_node_->create_publisher<std_msgs::msg::String>("path_handler_selector", qos);
 
  main_layout_ = new QVBoxLayout;
  row_1_label_layout_ = new QHBoxLayout;
  row_2_label_layout_ = new QHBoxLayout;
  row_3_label_layout_ = new QHBoxLayout;
  row_1_layout_ = new QHBoxLayout;
  row_2_layout_ = new QHBoxLayout;
  row_3_layout_ = new QHBoxLayout;
  controller_ = new QComboBox;
  planner_ = new QComboBox;
  goal_checker_ = new QComboBox;
  smoother_ = new QComboBox;
  progress_checker_ = new QComboBox;
  path_handler_ = new QComboBox;
 
  main_layout_->setContentsMargins(10, 10, 10, 10);
 
  row_1_label_layout_->addWidget(new QLabel("Controller"));
  row_1_layout_->addWidget(controller_);
  row_1_label_layout_->addWidget(new QLabel("Planner"));
  row_1_layout_->addWidget(planner_);
  row_2_label_layout_->addWidget(new QLabel("Goal Checker"));
  row_2_layout_->addWidget(goal_checker_);
  row_2_label_layout_->addWidget(new QLabel("Smoother"));
  row_2_layout_->addWidget(smoother_);
  row_3_label_layout_->addWidget(new QLabel("Progress Checker"));
  row_3_layout_->addWidget(progress_checker_);
  row_3_label_layout_->addWidget(new QLabel("Path Handler"));
  row_3_layout_->addWidget(path_handler_);
 
  main_layout_->addLayout(row_1_label_layout_);
  main_layout_->addLayout(row_1_layout_);
  main_layout_->addLayout(row_2_label_layout_);
  main_layout_->addLayout(row_2_layout_);
  main_layout_->addLayout(row_3_label_layout_);
  main_layout_->addLayout(row_3_layout_);
 
  setLayout(main_layout_);
  loadPlugins();
 
  connect(
    controller_, QOverload<int>::of(&QComboBox::activated), this,
    &Selector::setController);
 
  connect(
    planner_, QOverload<int>::of(&QComboBox::activated), this,
    &Selector::setPlanner);
 
  connect(
    goal_checker_, QOverload<int>::of(&QComboBox::activated), this,
    &Selector::setGoalChecker);
 
  connect(
    smoother_, QOverload<int>::of(&QComboBox::activated), this,
    &Selector::setSmoother);
 
  connect(
    progress_checker_, QOverload<int>::of(&QComboBox::activated), this,
    &Selector::setProgressChecker);
 
  connect(
    path_handler_, QOverload<int>::of(&QComboBox::activated), this,
    &Selector::setPathHandler);
}
 
Selector::~Selector()
{
  if (load_plugins_thread_.joinable()) {
    load_plugins_thread_.join();
  }
}
 
// Publish the selected controller or planner
void Selector::setSelection(
  QComboBox * combo_box,
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher)  //  nosemgrep
{
  // If "default" option is selected, it gets removed and the next item is selected
  if (combo_box->findText("Default") != -1) {
    combo_box->removeItem(0);
  }
 
  // if there are no plugins available, return
  if (combo_box->count() == 0) {
    return;
  }
 
  auto msg = std::make_unique<std_msgs::msg::String>();
  msg->data = combo_box->currentText().toStdString();
 
  publisher->publish(std::move(msg));
}
 
// Call setSelection() for controller
void Selector::setController()
{
  setSelection(controller_, pub_controller_);
}
 
// Call setSelection() for planner
void Selector::setPlanner()
{
  setSelection(planner_, pub_planner_);
}
 
// Call setSelection() for goal checker
void Selector::setGoalChecker()
{
  setSelection(goal_checker_, pub_goal_checker_);
}
 
// Call setSelection() for smoother
void Selector::setSmoother()
{
  setSelection(smoother_, pub_smoother_);
}
 
void Selector::setProgressChecker()
{
  setSelection(progress_checker_, pub_progress_checker_);
}
 
void Selector::setPathHandler()
{
  setSelection(path_handler_, pub_path_handler_);
}
 
void
Selector::loadPlugins()
{
  load_plugins_thread_ = std::thread(
    [this]() {
      rclcpp::Rate rate(0.2);
      while (rclcpp::ok() && !plugins_loaded_) {
        RCLCPP_INFO(client_node_->get_logger(), "Trying to load 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_);
        nav2_rviz_plugins::pluginLoader(
          client_node_, server_failed_, "controller_server", "path_handler_plugins",
          path_handler_);
        if (controller_->count() > 0 &&
        planner_->count() > 0 &&
        goal_checker_->count() > 0 &&
        smoother_->count() > 0 &&
        progress_checker_->count() > 0 &&
        path_handler_->count() > 0)
        {
          plugins_loaded_ = true;
        } else {
          RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying...");
        }
        rate.sleep();
      }
    });
}
 
}  // namespace nav2_rviz_plugins
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::Selector, rviz_common::Panel)