mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
01d43e3f3b
commit
3e788084b2
@ -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;
|
||||
|
||||
@ -184,16 +184,11 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_const
|
||||
const std::string id = event.getPublisherName() + "/" + msg->task_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();
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -44,6 +44,7 @@
|
||||
#include "factory_model.h"
|
||||
#include "pluginlib_factory.h"
|
||||
#include "task_display.h"
|
||||
#include <moveit/visualization_tools/task_solution_visualization.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
|
||||
#include <rviz/properties/property.h>
|
||||
@ -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();
|
||||
for (const auto& index : selected_rows) {
|
||||
const DisplaySolutionPtr &solution = task->getSolution(index);
|
||||
display->showMarkers(solution);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
#include <rviz/panel.h>
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <QModelIndex>
|
||||
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);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -121,6 +121,9 @@
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="selectionMode">
|
||||
<enum>QAbstractItemView::ExtendedSelection</enum>
|
||||
</property>
|
||||
<property name="rootIsDecorated">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
|
||||
@ -43,6 +43,7 @@
|
||||
|
||||
#include <rviz/panel.h>
|
||||
#include <rviz/properties/property_tree_model.h>
|
||||
#include <QPointer>
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
@ -64,8 +65,12 @@ public:
|
||||
inline std::pair<BaseTaskModel*, QModelIndex>
|
||||
getTaskModel(const QModelIndex& index) const;
|
||||
|
||||
/// unlock locked_display_ if given display is different
|
||||
void unlock(TaskDisplay *display);
|
||||
|
||||
TaskPanel* q_ptr;
|
||||
rviz::PropertyTreeModel* settings;
|
||||
QPointer<TaskDisplay> locked_display_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -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<rviz::Robot*> 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_;
|
||||
|
||||
@ -65,8 +65,10 @@ MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::M
|
||||
|
||||
MarkerVisualization::~MarkerVisualization()
|
||||
{
|
||||
for (const auto& pair : namespaces_)
|
||||
for (const auto& pair : namespaces_) {
|
||||
if (pair.second)
|
||||
pair.second->getCreator()->destroySceneNode(pair.second);
|
||||
}
|
||||
}
|
||||
|
||||
void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible)
|
||||
|
||||
@ -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)
|
||||
{
|
||||
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);
|
||||
const planning_scene::PlanningSceneConstPtr &scene = displaying_solution_->scene(idx_pair);
|
||||
scene = displaying_solution_->scene(idx_pair);
|
||||
|
||||
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())
|
||||
|
||||
Loading…
Reference in New Issue
Block a user