Skip to content

Commit

Permalink
updating the changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@padhupradheep
padhupradheep committed Feb 1, 2024
1 parent a67d64d commit 7606582
Show file tree
Hide file tree
Showing 2 changed files with 114 additions and 124 deletions.
80 changes: 47 additions & 33 deletions nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp
View file
Original file line number Diff line number Diff line change
@@ -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 <QtWidgets>
#include "vector"
#include "memory"
#include "string"
#include <QBasicTimer>
#include <QFrame>
#include <QGridLayout>
#include <QParallelAnimationGroup>
#include <QScrollArea>
#include <QToolButton>
#include <QWidget>

#include "rclcpp/rclcpp.hpp"
#include "rviz_common/panel.hpp"
#include "vector"
#include "memory"
#include "string"
#include "std_msgs/msg/string.hpp"

class QPushButton;
Expand All @@ -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<std_msgs::msg::String>::SharedPtr pub_controller_;
std::string controller_name_;
rclcpp::Publisher<std_msgs::msg::String>::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_
#endif // NAV2_RVIZ_PLUGINS__SELECTOR_HPP_
158 changes: 67 additions & 91 deletions nav2_rviz_plugins/src/selector.cpp
View file
Original file line number Diff line number Diff line change
@@ -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 <QImage>

using namespace std::chrono_literals;

Expand All @@ -9,136 +22,100 @@ namespace nav2_rviz_plugins
Selector::Selector(QWidget * parent)
: Panel(parent)
{
client_node_ = std::make_shared<rclcpp::Node>("example");
client_node_ = std::make_shared<rclcpp::Node>("nav2_rviz_selector_node");
rclcpp::QoS qos(rclcpp::KeepLast(1));
qos.transient_local().reliable();

pub_controller_ = client_node_->create_publisher<std_msgs::msg::String>("controller_selector", qos);
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);

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<int>::of(&QComboBox::activated), this,
controller_, QOverload<int>::of(&QComboBox::activated), this,
&Selector::setController);

connect(
planner_, QOverload<int>::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<rclcpp::SyncParametersClient>(client_node_, "controller_server");
QStringList id_list;

while (!parameter_client_con->wait_for_service(1s)) {
auto parameter_client = std::make_shared<rclcpp::SyncParametersClient>(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<<parameters_con.size()<<std::endl;
// auto str_arr = parameters_con[0].as_string_array();
// for (auto str: str_arr)
// id_list.push_back(QString::fromStdString(str));
// controller_params->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 &param_name : parameters.names) {
std::cout << param_name << std::endl;
}
auto parameter_client = std::make_shared<rclcpp::SyncParametersClient>(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
Expand All @@ -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)
PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::Selector, rviz_common::Panel)

0 comments on commit 7606582

Please sign in to comment.