From 3e788084b2c5d4ff402025936b1370a9c026764c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 8 Jan 2018 01:41:05 +0100 Subject: [PATCH] improve solution visualization / navigation - locking: suppress interrupting current solution display when navigating solutions - display end scene at the end of a solution display - allow to display multiple solutions (its markers) --- .../src/remote_task_model.cpp | 4 +- .../src/task_display.cpp | 7 +- .../motion_planning_tasks/src/task_display.h | 2 +- .../motion_planning_tasks/src/task_panel.cpp | 55 +++++++++++---- .../motion_planning_tasks/src/task_panel.h | 2 + .../motion_planning_tasks/src/task_panel.ui | 3 + .../motion_planning_tasks/src/task_panel_p.h | 5 ++ .../task_solution_visualization.h | 5 +- .../src/marker_visualization.cpp | 6 +- .../src/task_solution_visualization.cpp | 69 ++++++++++--------- 10 files changed, 101 insertions(+), 57 deletions(-) diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 403877c9..78ecfe37 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -355,7 +355,9 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index) moveit_task_constructor_msgs::GetSolution srv; srv.request.solution_id = id; if (get_solution_client_->call(srv)) { - result = processSolutionMessage(srv.response.solution); + id_to_solution_[id] = result = processSolutionMessage(srv.response.solution); + } else { // on failure mark remote task as destroyed: don't retrieve more solutions + flags_ |= IS_DESTROYED; } } return result; diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index c6af95b6..6891e395 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -184,16 +184,11 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEventtask_id; mainloop_jobs_.addJob([this, id, msg]() { const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg); - if (s) trajectory_visual_->showTrajectory(s); + if (s) trajectory_visual_->showTrajectory(s, false); return; }); } -void TaskDisplay::showTrajectory(const DisplaySolutionPtr &s) const { - trajectory_visual_->interruptCurrentDisplay(); - trajectory_visual_->showTrajectory(s); -} - void TaskDisplay::clearMarkers() { marker_visual_->clearMarkers(); } diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index 3ee11aa4..4d928a54 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -87,7 +87,7 @@ public: void setName(const QString& name); TaskListModel& getTaskListModel() { return *task_list_model_; } - void showTrajectory(const DisplaySolutionPtr& s) const; + TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); } void clearMarkers(); void showMarkers(const DisplaySolutionPtr &s); diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 7abc7569..e37c2abe 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -44,6 +44,7 @@ #include "factory_model.h" #include "pluginlib_factory.h" #include "task_display.h" +#include #include #include @@ -134,6 +135,15 @@ TaskPanelPrivate::getTaskModel(const QModelIndex &index) const return meta_model->getTaskModel(index); } +void TaskPanelPrivate::unlock(TaskDisplay* display) +{ + if (locked_display_ && locked_display_ != display) { + locked_display_->clearMarkers(); + locked_display_->visualization()->unlock(); + locked_display_ = display; + } +} + void TaskPanel::onInitialize() { } @@ -189,6 +199,9 @@ void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIn std::tie(task, task_index) = d_ptr->getTaskModel(current); d_ptr->actionRemoveTaskTreeRows->setEnabled(task != nullptr); + // TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second; + d_ptr->unlock(nullptr); + auto *view = d_ptr->solutions_view; QItemSelectionModel *sm = view->selectionModel(); QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr; @@ -196,31 +209,47 @@ void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIn view->setModel(m); delete sm; // we don't store the selection model - if (m) - connect(view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), + if (m) { + sm = view->selectionModel(); + connect(sm, SIGNAL(currentChanged(QModelIndex, QModelIndex)), this, SLOT(onCurrentSolutionChanged(QModelIndex,QModelIndex))); + connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)), + this, SLOT(onSolutionSelectionChanged(QItemSelection, QItemSelection))); + } } void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous) { - if (!current.isValid()) + TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second; + d_ptr->unlock(display); + + if (!display || !current.isValid()) return; + BaseTaskModel *task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first; + Q_ASSERT(task); + + d_ptr->locked_display_ = display; + TaskSolutionVisualization* vis = display->visualization(); + vis->interruptCurrentDisplay(); + vis->showTrajectory(task->getSolution(current), true); +} + +void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected) +{ + QItemSelectionModel *sm = d_ptr->solutions_view->selectionModel(); + const QModelIndexList& selected_rows = sm->selectedRows(); + TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second; Q_ASSERT(display); - - BaseTaskModel *task; - QModelIndex task_index; - std::tie(task, task_index) = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()); + BaseTaskModel *task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first; Q_ASSERT(task); - const DisplaySolutionPtr &solution = task->getSolution(current); - if (!solution) - return; - - display->showTrajectory(solution); display->clearMarkers(); - display->showMarkers(solution); + for (const auto& index : selected_rows) { + const DisplaySolutionPtr &solution = task->getSolution(index); + display->showMarkers(solution); + } } } diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index 6eddca97..d7ac8d2b 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -41,6 +41,7 @@ #include #include #include +class QItemSelection; namespace moveit_rviz_plugin { @@ -77,6 +78,7 @@ protected Q_SLOTS: void removeSelectedStages(); void onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous); void onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous); + void onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected); }; } diff --git a/visualization/motion_planning_tasks/src/task_panel.ui b/visualization/motion_planning_tasks/src/task_panel.ui index b7ed1792..7fa9a4d6 100644 --- a/visualization/motion_planning_tasks/src/task_panel.ui +++ b/visualization/motion_planning_tasks/src/task_panel.ui @@ -121,6 +121,9 @@ 0 + + QAbstractItemView::ExtendedSelection + false diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index fa840f8e..0f67114c 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -43,6 +43,7 @@ #include #include +#include namespace moveit_rviz_plugin { @@ -64,8 +65,12 @@ public: inline std::pair getTaskModel(const QModelIndex& index) const; + /// unlock locked_display_ if given display is different + void unlock(TaskDisplay *display); + TaskPanel* q_ptr; rviz::PropertyTreeModel* settings; + QPointer locked_display_; }; } diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index fef7efb8..58a71ade 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -105,8 +105,8 @@ public: planning_scene::PlanningSceneConstPtr getScene() const { return scene_; } void showTrajectory(const moveit_task_constructor_msgs::Solution& msg); - void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s); - void dropTrajectory(); + void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s, bool lock); + void unlock(); public Q_SLOTS: void interruptCurrentDisplay(); @@ -148,6 +148,7 @@ protected: std::vector trail_; bool animating_ = false; bool drop_displaying_solution_ = false; + bool locked_ = false; int current_state_ = -1; float current_state_time_; boost::mutex display_solution_mutex_; diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index 345a2878..2ae67417 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -65,8 +65,10 @@ MarkerVisualization::MarkerVisualization(const std::vectorgetCreator()->destroySceneNode(pair.second); + for (const auto& pair : namespaces_) { + if (pair.second) + pair.second->getCreator()->destroySceneNode(pair.second); + } } void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible) diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 03366b30..a73a29a7 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -326,11 +326,8 @@ void TaskSolutionVisualization::onDisable() void TaskSolutionVisualization::interruptCurrentDisplay() { - // update() starts a new trajectory as soon as it is available - // interrupting may cause the newly received trajectory to interrupt - // hence, only interrupt when current_state_ already advanced past first - if (current_state_ > 0) - animating_ = false; + if (!locked_) + drop_displaying_solution_ = true; } float TaskSolutionVisualization::getStateDisplayTime() @@ -355,11 +352,6 @@ float TaskSolutionVisualization::getStateDisplayTime() } } -void TaskSolutionVisualization::dropTrajectory() -{ - drop_displaying_solution_ = true; -} - void TaskSolutionVisualization::update(float wall_dt, float ros_dt) { if (drop_displaying_solution_) @@ -374,25 +366,23 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) boost::mutex::scoped_lock lock(display_solution_mutex_); // new trajectory available to display? - if (solution_to_display_ && !solution_to_display_->empty()) - { + if (solution_to_display_ && (!locked_ || !displaying_solution_)) { animating_ = true; displaying_solution_ = solution_to_display_; changedTrail(); if (slider_panel_) slider_panel_->update(solution_to_display_->getWayPointCount()); } - else if (displaying_solution_) - { + else if (displaying_solution_) { if (loop_display_property_->getBool()) { animating_ = true; - } else if (slider_panel_ && slider_panel_->isVisible()) - { + } else if (slider_panel_ && slider_panel_->isVisible()) { if (slider_panel_->getSliderPosition() == (int)displaying_solution_->getWayPointCount() - 1) return; // nothing more to do else animating_ = true; - } + } else if (locked_) + return; } solution_to_display_.reset(); @@ -435,29 +425,37 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) if (!loop_display_property_->getBool() && slider_panel_) slider_panel_->pauseButton(true); // ensure to render end state - renderWayPoint(waypoint_count - 1, previous_state); - renderPlanningScene(displaying_solution_->scene(waypoint_count)); + renderWayPoint(waypoint_count, previous_state); } current_state_time_ = 0.0f; } current_state_time_ += wall_dt; } - // main scene node is visible if: animation, trail, or panel is shown + // main scene node is visible if animation, trail, or panel is shown, or if display is locked setVisibility(main_scene_node_, parent_scene_node_, display_->isEnabled() && displaying_solution_ && - (animating_ || trail_scene_node_->getParent() || + (animating_ || locked_ || trail_scene_node_->getParent() || (slider_panel_ && slider_panel_->isVisible()))); } void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) { - auto idx_pair = displaying_solution_->indexPair(index); - const planning_scene::PlanningSceneConstPtr &scene = displaying_solution_->scene(idx_pair); + moveit::core::RobotStateConstPtr robot_state; + planning_scene::PlanningSceneConstPtr scene; + if (index == displaying_solution_->getWayPointCount()) { + scene = displaying_solution_->scene(index); + renderPlanningScene (scene); + robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); + } else { + auto idx_pair = displaying_solution_->indexPair(index); + scene = displaying_solution_->scene(idx_pair); - if (previous_index < 0 || - displaying_solution_->indexPair(previous_index).first != idx_pair.first) - renderPlanningScene (scene); + if (previous_index < 0 || + displaying_solution_->indexPair(previous_index).first != idx_pair.first) + renderPlanningScene (scene); + robot_state = displaying_solution_->getWayPointPtr(idx_pair); + } QColor attached_color = attached_body_color_property_->getColor(); std_msgs::ColorRGBA color; @@ -468,9 +466,9 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) planning_scene::ObjectColorMap color_map; scene->getKnownObjectColors(color_map); - robot_render_->update(displaying_solution_->getWayPointPtr(idx_pair), color, color_map); + robot_render_->update(robot_state, color, color_map); - if (slider_panel_) + if (slider_panel_ && index >= 0) slider_panel_->setSliderPosition(index); } @@ -495,19 +493,26 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msg DisplaySolutionPtr s (new DisplaySolution); s->setFromMessage(scene_, msg); - showTrajectory(s); + showTrajectory(s, false); } -void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s) +void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s, bool lock_display) { - if (!s->empty()) { + if (lock_display || !s->empty()) { boost::mutex::scoped_lock lock(display_solution_mutex_); solution_to_display_ = s; - if (interrupt_display_property_->getBool()) + if (lock_display) + locked_ = drop_displaying_solution_ = true; + else if (interrupt_display_property_->getBool()) interruptCurrentDisplay(); } } +void TaskSolutionVisualization::unlock() +{ + locked_ = false; +} + void TaskSolutionVisualization::changedRobotColor() { if (enable_robot_color_property_->getBool())