39#include <moveit_msgs/msg/joint_limits.hpp>
41#include <QApplication>
42#include <QDoubleValidator>
51#include <QStackedWidget>
53#include <QTableWidget>
55#include <QTreeWidgetItem>
71 QVBoxLayout* layout =
new QVBoxLayout();
73 layout->setAlignment(Qt::AlignTop);
76 setWindowTitle(
"Controller Configuration");
80 layout->addWidget(header);
87 connect(
joints_widget_, SIGNAL(cancelEditing()),
this, SLOT(cancelEditing()));
88 connect(
joints_widget_, SIGNAL(doneEditing()),
this, SLOT(saveJointsScreen()));
89 connect(
joints_widget_, SIGNAL(previewSelected(std::vector<std::string>)),
this,
90 SLOT(previewSelectedJoints(std::vector<std::string>)));
97 SLOT(previewSelectedGroup(std::vector<std::string>)));
124 QWidget* content_widget =
new QWidget(
this);
127 QVBoxLayout* layout =
new QVBoxLayout(
this);
129 QHBoxLayout* upper_controls_layout =
new QHBoxLayout();
132 QPushButton* btn_add_default =
new QPushButton(
setup_step_->getButtonText().c_str(),
this);
133 btn_add_default->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
134 btn_add_default->setMaximumWidth(600);
135 connect(btn_add_default, SIGNAL(clicked()),
this, SLOT(addDefaultControllers()));
136 upper_controls_layout->addWidget(btn_add_default);
137 upper_controls_layout->setAlignment(btn_add_default, Qt::AlignLeft);
140 layout->addLayout(upper_controls_layout);
146 labels <<
"Controller"
147 <<
"Controller Type";
150 connect(
controllers_tree_, SIGNAL(itemDoubleClicked(QTreeWidgetItem*,
int)),
this, SLOT(editSelected()));
152 SLOT(previewSelected(QTreeWidgetItem*,
int)));
153 connect(
controllers_tree_, SIGNAL(itemSelectionChanged()),
this, SLOT(itemSelectionChanged()));
161 QLabel* expand_controls =
new QLabel(
this);
162 expand_controls->setText(
"<a href='expand'>Expand All</a> <a href='contract'>Collapse All</a>");
163 connect(expand_controls, SIGNAL(linkActivated(
const QString)),
this, SLOT(alterTree(
const QString)));
167 controls_layout_->addItem(
new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Minimum));
170 btn_delete_ =
new QPushButton(
"&Delete Controller",
this);
171 btn_delete_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
173 connect(
btn_delete_, SIGNAL(clicked()),
this, SLOT(deleteController()));
178 btn_add_ =
new QPushButton(
"&Add Controller",
this);
180 connect(
btn_add_, SIGNAL(clicked()),
this, SLOT(addController()));
185 btn_edit_ =
new QPushButton(
"&Edit Selected",
this);
187 connect(
btn_edit_, SIGNAL(clicked()),
this, SLOT(editSelected()));
195 content_widget->setLayout(layout);
197 return content_widget;
229 const QFont top_level_font(QFont().defaultFamily(), 11, QFont::Bold);
230 const QFont type_font(QFont().defaultFamily(), 11, QFont::Normal, QFont::StyleItalic);
232 QTreeWidgetItem* controller;
234 controller =
new QTreeWidgetItem();
237 controller->setText(0, controller_it.
name_.c_str());
238 controller->setFont(0, top_level_font);
239 controller->setData(0, Qt::UserRole, QVariant::fromValue(0));
242 controller->setText(1, controller_it.
type_.c_str());
243 controller->setFont(1, type_font);
244 controller->setData(1, Qt::UserRole, QVariant::fromValue(4));
247 if (!controller_it.
joints_.empty())
250 QTreeWidgetItem* joints =
new QTreeWidgetItem(controller);
251 joints->setText(0,
"Joints");
252 joints->setFont(0, type_font);
253 joints->setData(0, Qt::UserRole, QVariant::fromValue(1));
254 controller->addChild(joints);
257 for (
const std::string& joint : controller_it.
joints_)
259 QTreeWidgetItem* joint_item =
new QTreeWidgetItem(joints);
260 joint_item->setData(0, Qt::UserRole, QVariant::fromValue(2));
263 joint_item->setText(0, joint.c_str());
264 joints->addChild(joint_item);
286 const std::vector<std::string>& joints =
setup_step_->getJointNames();
290 QMessageBox::critical(
this,
"Error Loading",
"No joints found for robot model");
302 QString(
"Edit '").append(QString::fromUtf8(this_controller->
name_.c_str())).append(
"' Joint Collection"));
321 QString(
"Edit '").append(QString::fromUtf8(this_controller->
name_.c_str())).append(
"' Joints groups collection"));
330void ControllersWidget::deleteController()
334 if (controller_name.empty())
342 int type = item->data(0, Qt::UserRole).value<
int>();
344 controller_name = item->text(0).toUtf8().constData();
348 if (QMessageBox::question(
349 this,
"Confirm Controller Deletion",
350 QString(
"Are you sure you want to delete the controller '").append(controller_name.c_str()).append(
" ?"),
351 QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
356 if (
setup_step_->deleteController(controller_name))
358 RCLCPP_INFO_STREAM(
setup_step_->getLogger(),
"Controller " << controller_name <<
" deleted successfully");
362 RCLCPP_WARN_STREAM(
setup_step_->getLogger(),
"Couldn't delete Controller " << controller_name);
377void ControllersWidget::addController()
390void ControllersWidget::addDefaultControllers()
393 QMessageBox::warning(
this,
"Error adding controllers",
"No Planning Groups configured!");
405 if (this_controller ==
nullptr)
430void ControllersWidget::cancelEditing()
435 if (editing && editing->
joints_.empty())
456void ControllersWidget::previewSelectedJoints(
const std::vector<std::string>& joints)
461 for (
const std::string& joint : joints)
464 const std::string link =
setup_step_->getChildOfJoint(joint);
479void ControllersWidget::previewSelectedGroup(
const std::vector<std::string>& groups)
484 for (
const std::string& group : groups)
494void ControllersWidget::previewSelected(QTreeWidgetItem* selected_item,
int )
497 int type = selected_item->data(0, Qt::UserRole).value<
int>();
514void ControllersWidget::saveControllerScreenJoints()
517 if (!saveControllerScreen())
532void ControllersWidget::saveControllerScreenGroups()
535 if (!saveControllerScreen())
550void ControllersWidget::saveJointsScreen()
556 searched_controller->joints_.clear();
574void ControllersWidget::saveJointsGroupsScreen()
592void ControllersWidget::saveControllerScreenEdit()
595 if (!saveControllerScreen())
605bool ControllersWidget::saveControllerScreen()
613 ControllerInfo* searched_controller =
nullptr;
615 std::smatch invalid_name_match;
616 std::regex invalid_reg_ex(
"[^a-z|^1-9|^_]");
619 if (controller_name.empty() || std::regex_search(controller_name, invalid_name_match, invalid_reg_ex))
621 QMessageBox::warning(
this,
"Error Saving",
"Invalid controller name");
633 for (
const auto& controller :
setup_step_->getControllers())
635 if (controller.name_.compare(controller_name) == 0)
638 if (&controller != searched_controller)
640 QMessageBox::warning(
this,
"Error Saving",
"A controller already exists with that name!");
649 if (searched_controller ==
nullptr)
651 ControllerInfo new_controller;
652 new_controller.name_ = controller_name;
653 new_controller.type_ = controller_type;
654 new_controller.parameters_ = controller_parameters;
662 const std::string old_controller_name = searched_controller->name_;
665 searched_controller->name_ = controller_name;
666 searched_controller->type_ = controller_type;
667 searched_controller->parameters_ = controller_parameters;
682void ControllersWidget::editSelected()
685 QTreeWidgetItem* controller_item;
694 int type = item->data(0, Qt::UserRole).value<
int>();
699 controller_item = item->parent()->parent();
711 controller_item = item->parent();
733 QMessageBox::critical(
this,
"Error Loading",
"An internal error has occurred while loading.");
740void ControllersWidget::editController()
781void ControllersWidget::alterTree(
const QString& link)
783 if (link.contains(
"expand"))
793void ControllersWidget::itemSelectionChanged()
796 if (selected_items.empty())
void highlightGroup(const std::string &group_name)
void highlightLink(const std::string &link_name, const QColor &color)
std::vector< std::string > joints_