mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
TaskPanel: execute selected solution
This commit is contained in:
parent
ba815cff34
commit
ef6cb1a746
@ -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);
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
};
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user