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)
This commit is contained in:
Robert Haschke 2018-01-08 01:41:05 +01:00
parent 01d43e3f3b
commit 3e788084b2
10 changed files with 101 additions and 57 deletions

View File

@ -355,7 +355,9 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
moveit_task_constructor_msgs::GetSolution srv; moveit_task_constructor_msgs::GetSolution srv;
srv.request.solution_id = id; srv.request.solution_id = id;
if (get_solution_client_->call(srv)) { 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; return result;

View File

@ -184,16 +184,11 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_const
const std::string id = event.getPublisherName() + "/" + msg->task_id; const std::string id = event.getPublisherName() + "/" + msg->task_id;
mainloop_jobs_.addJob([this, id, msg]() { mainloop_jobs_.addJob([this, id, msg]() {
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(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; return;
}); });
} }
void TaskDisplay::showTrajectory(const DisplaySolutionPtr &s) const {
trajectory_visual_->interruptCurrentDisplay();
trajectory_visual_->showTrajectory(s);
}
void TaskDisplay::clearMarkers() { void TaskDisplay::clearMarkers() {
marker_visual_->clearMarkers(); marker_visual_->clearMarkers();
} }

View File

@ -87,7 +87,7 @@ public:
void setName(const QString& name); void setName(const QString& name);
TaskListModel& getTaskListModel() { return *task_list_model_; } TaskListModel& getTaskListModel() { return *task_list_model_; }
void showTrajectory(const DisplaySolutionPtr& s) const; TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); }
void clearMarkers(); void clearMarkers();
void showMarkers(const DisplaySolutionPtr &s); void showMarkers(const DisplaySolutionPtr &s);

View File

@ -44,6 +44,7 @@
#include "factory_model.h" #include "factory_model.h"
#include "pluginlib_factory.h" #include "pluginlib_factory.h"
#include "task_display.h" #include "task_display.h"
#include <moveit/visualization_tools/task_solution_visualization.h>
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <rviz/properties/property.h> #include <rviz/properties/property.h>
@ -134,6 +135,15 @@ TaskPanelPrivate::getTaskModel(const QModelIndex &index) const
return meta_model->getTaskModel(index); 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() void TaskPanel::onInitialize()
{ {
} }
@ -189,6 +199,9 @@ void TaskPanel::onCurrentStageChanged(const QModelIndex &current, const QModelIn
std::tie(task, task_index) = d_ptr->getTaskModel(current); std::tie(task, task_index) = d_ptr->getTaskModel(current);
d_ptr->actionRemoveTaskTreeRows->setEnabled(task != nullptr); 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; auto *view = d_ptr->solutions_view;
QItemSelectionModel *sm = view->selectionModel(); QItemSelectionModel *sm = view->selectionModel();
QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr; QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr;
@ -196,32 +209,48 @@ void TaskPanel::onCurrentStageChanged(const QModelIndex &current, const QModelIn
view->setModel(m); view->setModel(m);
delete sm; // we don't store the selection model delete sm; // we don't store the selection model
if (m) if (m) {
connect(view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), sm = view->selectionModel();
connect(sm, SIGNAL(currentChanged(QModelIndex, QModelIndex)),
this, SLOT(onCurrentSolutionChanged(QModelIndex,QModelIndex))); this, SLOT(onCurrentSolutionChanged(QModelIndex,QModelIndex)));
connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)),
this, SLOT(onSolutionSelectionChanged(QItemSelection, QItemSelection)));
}
} }
void TaskPanel::onCurrentSolutionChanged(const QModelIndex &current, const QModelIndex &previous) void TaskPanel::onCurrentSolutionChanged(const QModelIndex &current, 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; 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; TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second;
Q_ASSERT(display); Q_ASSERT(display);
BaseTaskModel *task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first;
BaseTaskModel *task;
QModelIndex task_index;
std::tie(task, task_index) = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex());
Q_ASSERT(task); Q_ASSERT(task);
const DisplaySolutionPtr &solution = task->getSolution(current);
if (!solution)
return;
display->showTrajectory(solution);
display->clearMarkers(); display->clearMarkers();
for (const auto& index : selected_rows) {
const DisplaySolutionPtr &solution = task->getSolution(index);
display->showMarkers(solution); display->showMarkers(solution);
} }
}
} }

View File

@ -41,6 +41,7 @@
#include <rviz/panel.h> #include <rviz/panel.h>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <QModelIndex> #include <QModelIndex>
class QItemSelection;
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
@ -77,6 +78,7 @@ protected Q_SLOTS:
void removeSelectedStages(); void removeSelectedStages();
void onCurrentStageChanged(const QModelIndex &current, const QModelIndex &previous); void onCurrentStageChanged(const QModelIndex &current, const QModelIndex &previous);
void onCurrentSolutionChanged(const QModelIndex &current, const QModelIndex &previous); void onCurrentSolutionChanged(const QModelIndex &current, const QModelIndex &previous);
void onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected);
}; };
} }

View File

@ -121,6 +121,9 @@
<verstretch>0</verstretch> <verstretch>0</verstretch>
</sizepolicy> </sizepolicy>
</property> </property>
<property name="selectionMode">
<enum>QAbstractItemView::ExtendedSelection</enum>
</property>
<property name="rootIsDecorated"> <property name="rootIsDecorated">
<bool>false</bool> <bool>false</bool>
</property> </property>

View File

@ -43,6 +43,7 @@
#include <rviz/panel.h> #include <rviz/panel.h>
#include <rviz/properties/property_tree_model.h> #include <rviz/properties/property_tree_model.h>
#include <QPointer>
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
@ -64,8 +65,12 @@ public:
inline std::pair<BaseTaskModel*, QModelIndex> inline std::pair<BaseTaskModel*, QModelIndex>
getTaskModel(const QModelIndex& index) const; getTaskModel(const QModelIndex& index) const;
/// unlock locked_display_ if given display is different
void unlock(TaskDisplay *display);
TaskPanel* q_ptr; TaskPanel* q_ptr;
rviz::PropertyTreeModel* settings; rviz::PropertyTreeModel* settings;
QPointer<TaskDisplay> locked_display_;
}; };
} }

View File

@ -105,8 +105,8 @@ public:
planning_scene::PlanningSceneConstPtr getScene() const { return scene_; } planning_scene::PlanningSceneConstPtr getScene() const { return scene_; }
void showTrajectory(const moveit_task_constructor_msgs::Solution& msg); void showTrajectory(const moveit_task_constructor_msgs::Solution& msg);
void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s); void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s, bool lock);
void dropTrajectory(); void unlock();
public Q_SLOTS: public Q_SLOTS:
void interruptCurrentDisplay(); void interruptCurrentDisplay();
@ -148,6 +148,7 @@ protected:
std::vector<rviz::Robot*> trail_; std::vector<rviz::Robot*> trail_;
bool animating_ = false; bool animating_ = false;
bool drop_displaying_solution_ = false; bool drop_displaying_solution_ = false;
bool locked_ = false;
int current_state_ = -1; int current_state_ = -1;
float current_state_time_; float current_state_time_;
boost::mutex display_solution_mutex_; boost::mutex display_solution_mutex_;

View File

@ -65,9 +65,11 @@ MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::M
MarkerVisualization::~MarkerVisualization() MarkerVisualization::~MarkerVisualization()
{ {
for (const auto& pair : namespaces_) for (const auto& pair : namespaces_) {
if (pair.second)
pair.second->getCreator()->destroySceneNode(pair.second); pair.second->getCreator()->destroySceneNode(pair.second);
} }
}
void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible) void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible)
{ {

View File

@ -326,11 +326,8 @@ void TaskSolutionVisualization::onDisable()
void TaskSolutionVisualization::interruptCurrentDisplay() void TaskSolutionVisualization::interruptCurrentDisplay()
{ {
// update() starts a new trajectory as soon as it is available if (!locked_)
// interrupting may cause the newly received trajectory to interrupt drop_displaying_solution_ = true;
// hence, only interrupt when current_state_ already advanced past first
if (current_state_ > 0)
animating_ = false;
} }
float TaskSolutionVisualization::getStateDisplayTime() 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) void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
{ {
if (drop_displaying_solution_) 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_); boost::mutex::scoped_lock lock(display_solution_mutex_);
// new trajectory available to display? // new trajectory available to display?
if (solution_to_display_ && !solution_to_display_->empty()) if (solution_to_display_ && (!locked_ || !displaying_solution_)) {
{
animating_ = true; animating_ = true;
displaying_solution_ = solution_to_display_; displaying_solution_ = solution_to_display_;
changedTrail(); changedTrail();
if (slider_panel_) if (slider_panel_)
slider_panel_->update(solution_to_display_->getWayPointCount()); slider_panel_->update(solution_to_display_->getWayPointCount());
} }
else if (displaying_solution_) else if (displaying_solution_) {
{
if (loop_display_property_->getBool()) { if (loop_display_property_->getBool()) {
animating_ = true; 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) if (slider_panel_->getSliderPosition() == (int)displaying_solution_->getWayPointCount() - 1)
return; // nothing more to do return; // nothing more to do
else else
animating_ = true; animating_ = true;
} } else if (locked_)
return;
} }
solution_to_display_.reset(); solution_to_display_.reset();
@ -435,29 +425,37 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
if (!loop_display_property_->getBool() && slider_panel_) if (!loop_display_property_->getBool() && slider_panel_)
slider_panel_->pauseButton(true); slider_panel_->pauseButton(true);
// ensure to render end state // ensure to render end state
renderWayPoint(waypoint_count - 1, previous_state); renderWayPoint(waypoint_count, previous_state);
renderPlanningScene(displaying_solution_->scene(waypoint_count));
} }
current_state_time_ = 0.0f; current_state_time_ = 0.0f;
} }
current_state_time_ += wall_dt; 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_, setVisibility(main_scene_node_, parent_scene_node_,
display_->isEnabled() && displaying_solution_ && display_->isEnabled() && displaying_solution_ &&
(animating_ || trail_scene_node_->getParent() || (animating_ || locked_ || trail_scene_node_->getParent() ||
(slider_panel_ && slider_panel_->isVisible()))); (slider_panel_ && slider_panel_->isVisible())));
} }
void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
{ {
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); auto idx_pair = displaying_solution_->indexPair(index);
const planning_scene::PlanningSceneConstPtr &scene = displaying_solution_->scene(idx_pair); scene = displaying_solution_->scene(idx_pair);
if (previous_index < 0 || if (previous_index < 0 ||
displaying_solution_->indexPair(previous_index).first != idx_pair.first) displaying_solution_->indexPair(previous_index).first != idx_pair.first)
renderPlanningScene (scene); renderPlanningScene (scene);
robot_state = displaying_solution_->getWayPointPtr(idx_pair);
}
QColor attached_color = attached_body_color_property_->getColor(); QColor attached_color = attached_body_color_property_->getColor();
std_msgs::ColorRGBA color; std_msgs::ColorRGBA color;
@ -468,9 +466,9 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
planning_scene::ObjectColorMap color_map; planning_scene::ObjectColorMap color_map;
scene->getKnownObjectColors(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); slider_panel_->setSliderPosition(index);
} }
@ -495,19 +493,26 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msg
DisplaySolutionPtr s (new DisplaySolution); DisplaySolutionPtr s (new DisplaySolution);
s->setFromMessage(scene_, msg); 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_); boost::mutex::scoped_lock lock(display_solution_mutex_);
solution_to_display_ = s; solution_to_display_ = s;
if (interrupt_display_property_->getBool()) if (lock_display)
locked_ = drop_displaying_solution_ = true;
else if (interrupt_display_property_->getBool())
interruptCurrentDisplay(); interruptCurrentDisplay();
} }
} }
void TaskSolutionVisualization::unlock()
{
locked_ = false;
}
void TaskSolutionVisualization::changedRobotColor() void TaskSolutionVisualization::changedRobotColor()
{ {
if (enable_robot_color_property_->getBool()) if (enable_robot_color_property_->getBool())