From ef6cb1a746353bf2b11eb986c9f690e1c785e623 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 22 Feb 2019 19:45:51 +0100 Subject: [PATCH] TaskPanel: execute selected solution --- .../motion_planning_tasks/src/task_panel.cpp | 28 +++++++++++++++++-- .../motion_planning_tasks/src/task_panel.h | 1 + .../motion_planning_tasks/src/task_panel.ui | 16 +++++++++++ .../motion_planning_tasks/src/task_panel_p.h | 3 ++ .../visualization_tools/display_solution.h | 1 + .../src/display_solution.cpp | 11 ++++++++ 6 files changed, 58 insertions(+), 2 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index eab2ea1e..8ad91f6d 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -45,6 +45,7 @@ #include "pluginlib_factory.h" #include "task_display.h" #include +#include #include #include @@ -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); diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index 01a4de27..9496bcfd 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -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; diff --git a/visualization/motion_planning_tasks/src/task_panel.ui b/visualization/motion_planning_tasks/src/task_panel.ui index c0cf48a5..fc0b0797 100644 --- a/visualization/motion_planning_tasks/src/task_panel.ui +++ b/visualization/motion_planning_tasks/src/task_panel.ui @@ -41,8 +41,24 @@ + + + + Execute solution + + + Exec + + + + + + + + Show available stages + ... diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index dc979e48..5aadc513 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -42,6 +42,8 @@ #include "ui_task_panel.h" #include "ui_task_view.h" #include "ui_global_settings.h" +#include +#include #include #include @@ -81,6 +83,7 @@ public: TaskView* q_ptr; QPointer locked_display_; + actionlib::SimpleActionClient exec_action_client_; }; class GlobalSettingsWidgetPrivate : public Ui_GlobalSettingsWidget diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 30403ec2..97b2e20a 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -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; }; } diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index 55084cc3..92780045 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -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