TaskPanel: execute selected solution

This commit is contained in:
Robert Haschke 2019-02-22 19:45:51 +01:00
parent ba815cff34
commit ef6cb1a746
6 changed files with 58 additions and 2 deletions

View File

@ -45,6 +45,7 @@
#include "pluginlib_factory.h"
#include "task_display.h"
#include <moveit/visualization_tools/task_solution_visualization.h>
#include <moveit/visualization_tools/display_solution.h>
#include <moveit/task_constructor/stage.h>
#include <rviz/properties/property.h>
@ -91,8 +92,11 @@ TaskPanel::TaskPanel(QWidget* parent) : rviz::Panel(parent), d_ptr(new TaskPanel
connect(d->stackedWidget, &QStackedWidget::currentChanged, d->tool_buttons_group,
[d](int index) { d->tool_buttons_group->button(index)->setChecked(true); });
auto* task_view = new TaskView(this, d->property_root);
connect(d->button_exec_solution, SIGNAL(clicked()), task_view, SLOT(onExecCurrentSolution()));
// create sub widgets with corresponding tool buttons
addSubPanel(new TaskView(this, d->property_root), "Tasks View", QIcon(":/icons/tasks.png"));
addSubPanel(task_view, "Tasks View", QIcon(":/icons/tasks.png"));
d->stackedWidget->setCurrentIndex(0); // Tasks View is show by default
// settings widget should come last
@ -194,7 +198,7 @@ void setExpanded(QTreeView* view, const QModelIndex& index, bool expand, int dep
view->setExpanded(index, expand);
}
TaskViewPrivate::TaskViewPrivate(TaskView* q_ptr) : q_ptr(q_ptr) {
TaskViewPrivate::TaskViewPrivate(TaskView* q_ptr) : q_ptr(q_ptr), exec_action_client_("execute_task_solution") {
setupUi(q_ptr);
MetaTaskListModel* meta_model = &MetaTaskListModel::instance();
@ -450,6 +454,26 @@ void TaskView::onSolutionSelectionChanged(const QItemSelection& selected, const
}
}
void TaskView::onExecCurrentSolution() const {
const QModelIndex& current = d_ptr->solutions_view->currentIndex();
if (!current.isValid())
return;
BaseTaskModel* task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first;
Q_ASSERT(task);
const DisplaySolutionPtr& solution = task->getSolution(current);
if (!d_ptr->exec_action_client_.waitForServer(ros::Duration(0.1))) {
ROS_ERROR("Failed to connect to task execution action");
return;
}
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal;
solution->fillMessage(goal.solution);
d_ptr->exec_action_client_.sendGoal(goal);
}
GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz::Property* root)
: q_ptr(q_ptr) {
setupUi(q_ptr);

View File

@ -140,6 +140,7 @@ protected Q_SLOTS:
void onCurrentStageChanged(const QModelIndex& current, const QModelIndex& previous);
void onCurrentSolutionChanged(const QModelIndex& current, const QModelIndex& previous);
void onSolutionSelectionChanged(const QItemSelection& selected, const QItemSelection& deselected);
void onExecCurrentSolution() const;
};
class GlobalSettingsWidgetPrivate;

View File

@ -41,8 +41,24 @@
</property>
</spacer>
</item>
<item>
<widget class="QToolButton" name="button_exec_solution">
<property name="toolTip">
<string>Execute solution</string>
</property>
<property name="text">
<string>Exec</string>
</property>
<property name="icon">
<iconset theme="system-run"/>
</property>
</widget>
</item>
<item>
<widget class="QToolButton" name="button_show_stage_dock_widget">
<property name="toolTip">
<string>Show available stages</string>
</property>
<property name="text">
<string>...</string>
</property>

View File

@ -42,6 +42,8 @@
#include "ui_task_panel.h"
#include "ui_task_view.h"
#include "ui_global_settings.h"
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <actionlib/client/simple_action_client.h>
#include <rviz/panel.h>
#include <rviz/properties/property_tree_model.h>
@ -81,6 +83,7 @@ public:
TaskView* q_ptr;
QPointer<TaskDisplay> locked_display_;
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> exec_action_client_;
};
class GlobalSettingsWidgetPrivate : public Ui_GlobalSettingsWidget

View File

@ -124,5 +124,6 @@ public:
void setFromMessage(const planning_scene::PlanningScenePtr& start_scene,
const moveit_task_constructor_msgs::Solution& msg);
void fillMessage(moveit_task_constructor_msgs::Solution& msg) const;
};
}

View File

@ -123,4 +123,15 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta
}
}
void DisplaySolution::fillMessage(moveit_task_constructor_msgs::Solution& msg) const {
start_scene_->getPlanningSceneMsg(msg.start_scene);
msg.sub_trajectory.resize(data_.size());
auto traj_it = msg.sub_trajectory.begin();
for (const auto& sub : data_) {
sub.scene_->getPlanningSceneDiffMsg(traj_it->scene_diff);
sub.trajectory_->getRobotTrajectoryMsg(traj_it->trajectory);
++traj_it;
}
}
} // namespace moveit_rviz_plugin