highlight currently active solution part

This commit is contained in:
Robert Haschke 2018-02-25 23:49:06 +01:00
parent 0fd5f3c1b3
commit 3d07e34522
14 changed files with 92 additions and 2 deletions

View File

@ -221,6 +221,12 @@ bool LocalTaskModel::dropMimeData(const QMimeData *mime, Qt::DropAction action,
return true; return true;
} }
QModelIndex LocalTaskModel::indexFromStageId(size_t id) const
{
// TODO implement
return QModelIndex();
}
QAbstractItemModel *LocalTaskModel::getSolutionModel(const QModelIndex &index) QAbstractItemModel *LocalTaskModel::getSolutionModel(const QModelIndex &index)
{ {
// TODO implement // TODO implement

View File

@ -72,6 +72,8 @@ public:
void setStageFactory(const StageFactoryPtr &factory) override; void setStageFactory(const StageFactoryPtr &factory) override;
bool dropMimeData(const QMimeData *data, Qt::DropAction action, int row, int column, const QModelIndex &parent) override; bool dropMimeData(const QMimeData *data, Qt::DropAction action, int row, int column, const QModelIndex &parent) override;
QModelIndex indexFromStageId(size_t id) const override;
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
DisplaySolutionPtr getSolution(const QModelIndex &index) override; DisplaySolutionPtr getSolution(const QModelIndex &index) override;

View File

@ -99,7 +99,7 @@ bool MetaTaskListModel::removeRows(int row, int count, const QModelIndex &parent
{ {
if (!parent.isValid()) // forbid removal of top-level items (displays) if (!parent.isValid()) // forbid removal of top-level items (displays)
return false; return false;
TreeMergeProxyModel::removeRows(row, count, parent); return TreeMergeProxyModel::removeRows(row, count, parent);
} }
std::pair<TaskListModel*, TaskDisplay*> std::pair<TaskListModel*, TaskDisplay*>

View File

@ -220,6 +220,12 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i
return true; return true;
} }
QModelIndex RemoteTaskModel::indexFromStageId(size_t id) const
{
Node *n = node(id);
return n ? index(n) : QModelIndex();
}
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg) void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg)
{ {
// iterate over descriptions and create new / update existing nodes where needed // iterate over descriptions and create new / update existing nodes where needed

View File

@ -81,6 +81,7 @@ public:
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override;
QModelIndex indexFromStageId(size_t id) const override;
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg); void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg);
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg); void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg); DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg);

View File

@ -83,6 +83,8 @@ TaskDisplay::TaskDisplay() : Display()
this, SLOT(changedTaskSolutionTopic()), this); this, SLOT(changedTaskSolutionTopic()), this);
trajectory_visual_.reset(new TaskSolutionVisualization(this, this)); trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)),
task_list_model_.get(), SLOT(highlightStage(size_t)));
marker_visual_ = new MarkerVisualizationProperty("Markers", this); marker_visual_ = new MarkerVisualizationProperty("Markers", this);

View File

@ -212,6 +212,23 @@ Qt::ItemFlags TaskListModel::flags(const QModelIndex &index) const
return f; return f;
} }
void TaskListModel::highlightStage(size_t id)
{
if (!active_task_model_) return;
QModelIndex old_index = highlighted_row_index_;
QModelIndex new_index = active_task_model_->indexFromStageId(id);
if (new_index.isValid())
highlighted_row_index_ = new_index = mapFromSource(new_index);
else
highlighted_row_index_ = QModelIndex();
if (old_index == new_index) return;
if (old_index.isValid())
Q_EMIT dataChanged(old_index, old_index.sibling(old_index.row(), columnCount()-1));
if (new_index.isValid())
Q_EMIT dataChanged(new_index, new_index.sibling(new_index.row(), columnCount()-1));
}
QVariant TaskListModel::headerData(int section, Qt::Orientation orientation, int role) const QVariant TaskListModel::headerData(int section, Qt::Orientation orientation, int role) const
{ {
if (orientation == Qt::Horizontal) if (orientation == Qt::Horizontal)
@ -220,6 +237,15 @@ QVariant TaskListModel::headerData(int section, Qt::Orientation orientation, int
return QAbstractItemModel::headerData(section, orientation, role); return QAbstractItemModel::headerData(section, orientation, role);
} }
QVariant TaskListModel::data(const QModelIndex &index, int role) const
{
if (role == Qt::BackgroundRole && index.isValid() &&
index.row() == highlighted_row_index_.row() &&
index.parent() == highlighted_row_index_.parent())
return QColor(Qt::yellow);
return FlatMergeProxyModel::data(index, role);
}
// process a task description message: // process a task description message:
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one // update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
void TaskListModel::processTaskDescriptionMessage(const std::string& id, void TaskListModel::processTaskDescriptionMessage(const std::string& id,
@ -247,6 +273,9 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
// the model is managed by this instance via Qt's parent-child mechanism // the model is managed by this instance via Qt's parent-child mechanism
remote_task = new RemoteTaskModel(scene_, this); remote_task = new RemoteTaskModel(scene_, this);
remote_task->setSolutionClient(get_solution_client_); remote_task->setSolutionClient(get_solution_client_);
// HACK: always use the last created model as active
active_task_model_ = remote_task;
} }
if (!remote_task) if (!remote_task)
return; // task is not in use anymore return; // task is not in use anymore

View File

@ -48,6 +48,7 @@
#include <QAbstractItemModel> #include <QAbstractItemModel>
#include <QTreeView> #include <QTreeView>
#include <memory> #include <memory>
#include <QPointer>
namespace ros { class ServiceClient; } namespace ros { class ServiceClient; }
namespace rviz { class PropertyTreeModel; } namespace rviz { class PropertyTreeModel; }
@ -86,6 +87,9 @@ public:
unsigned int taskFlags() const { return flags_; } unsigned int taskFlags() const { return flags_; }
static QVariant flowIcon(moveit::task_constructor::InterfaceFlags f); static QVariant flowIcon(moveit::task_constructor::InterfaceFlags f);
/// retrieve model index associated with given stage id
virtual QModelIndex indexFromStageId(size_t id) const = 0;
/// get solution model for given stage index /// get solution model for given stage index
virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0; virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0;
/// get solution for given solution index /// get solution for given solution index
@ -119,6 +123,9 @@ class TaskListModel : public utils::FlatMergeProxyModel {
// factory used to create stages // factory used to create stages
StageFactoryPtr stage_factory_; StageFactoryPtr stage_factory_;
QPointer<BaseTaskModel> active_task_model_;
QPersistentModelIndex highlighted_row_index_;
void onRemoveModel(QAbstractItemModel *model) override; void onRemoveModel(QAbstractItemModel *model) override;
public: public:
@ -127,10 +134,12 @@ public:
void setScene(const planning_scene::PlanningSceneConstPtr& scene); void setScene(const planning_scene::PlanningSceneConstPtr& scene);
void setSolutionClient(ros::ServiceClient* client); void setSolutionClient(ros::ServiceClient* client);
void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; }
int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; } int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; }
static QVariant horizontalHeader(int column, int role); static QVariant horizontalHeader(int column, int role);
QVariant headerData(int section, Qt::Orientation orientation, int role) const override; QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
QVariant data(const QModelIndex &index, int role) const override;
/// process an incoming task description message - only call in Qt's main loop /// process an incoming task description message - only call in Qt's main loop
void processTaskDescriptionMessage(const std::string &id, const moveit_task_constructor_msgs::TaskDescription &msg); void processTaskDescriptionMessage(const std::string &id, const moveit_task_constructor_msgs::TaskDescription &msg);
@ -153,6 +162,9 @@ public:
bool dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent) override; bool dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent) override;
Qt::DropActions supportedDropActions() const override; Qt::DropActions supportedDropActions() const override;
Qt::ItemFlags flags(const QModelIndex &index) const override; Qt::ItemFlags flags(const QModelIndex &index) const override;
protected Q_SLOTS:
void highlightStage(size_t id);
}; };

View File

@ -261,6 +261,17 @@ QModelIndex FlatMergeProxyModel::parent(const QModelIndex &child) const
return d_ptr->mapFromSource(src_parent, data); return d_ptr->mapFromSource(src_parent, data);
} }
QModelIndex FlatMergeProxyModel::mapToSource(const QModelIndex &proxy_index) const
{
FlatMergeProxyModelPrivate::ModelData* data;
return d_ptr->mapToSource(proxy_index, data);
}
QModelIndex FlatMergeProxyModel::mapFromSource(const QModelIndex &src_index) const
{
return d_ptr->mapFromSource(src_index, nullptr);
}
Qt::ItemFlags FlatMergeProxyModel::flags(const QModelIndex &index) const Qt::ItemFlags FlatMergeProxyModel::flags(const QModelIndex &index) const
{ {
if (!index.isValid()) if (!index.isValid())

View File

@ -75,6 +75,9 @@ public:
QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override; QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override;
QModelIndex parent(const QModelIndex &index) const override; QModelIndex parent(const QModelIndex &index) const override;
QModelIndex mapToSource(const QModelIndex &proxy_index) const;
QModelIndex mapFromSource(const QModelIndex &src_index) const;
Qt::ItemFlags flags(const QModelIndex & index) const override; Qt::ItemFlags flags(const QModelIndex & index) const override;
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override;

View File

@ -76,6 +76,8 @@ class DisplaySolution
robot_trajectory::RobotTrajectoryPtr trajectory_; robot_trajectory::RobotTrajectoryPtr trajectory_;
/// optional name of the trajectory /// optional name of the trajectory
std::string name_; std::string name_;
/// id of creating stage
uint32_t creator_id_;
/// rviz markers /// rviz markers
MarkerVisualizationPtr markers_; MarkerVisualizationPtr markers_;
}; };
@ -91,6 +93,7 @@ public:
size_t getWayPointCount() const { return steps_; } size_t getWayPointCount() const { return steps_; }
bool empty() const { return steps_ == 0; } bool empty() const { return steps_ == 0; }
/// pair of trajectory part and way point index within part
typedef std::pair<size_t, size_t> IndexPair; typedef std::pair<size_t, size_t> IndexPair;
IndexPair indexPair(size_t index) const; IndexPair indexPair(size_t index) const;
@ -113,6 +116,8 @@ public:
const std::string& name(size_t index) const { const std::string& name(size_t index) const {
return name(indexPair(index)); return name(indexPair(index));
} }
uint32_t creatorId(const IndexPair& idx_pair) const;
const MarkerVisualizationPtr markers(const IndexPair& idx_pair) const; const MarkerVisualizationPtr markers(const IndexPair& idx_pair) const;
const MarkerVisualizationPtr markers(size_t index) const { const MarkerVisualizationPtr markers(size_t index) const {
return markers(indexPair(index)); return markers(indexPair(index));

View File

@ -127,6 +127,9 @@ private Q_SLOTS:
void changedSceneEnabled(); void changedSceneEnabled();
void renderCurrentScene(); void renderCurrentScene();
Q_SIGNALS:
void activeStageChanged(size_t);
protected: protected:
void setVisibility(); // set visibility of main scene node void setVisibility(); // set visibility of main scene node
void setVisibility(Ogre::SceneNode* node, Ogre::SceneNode* parent, bool visible); void setVisibility(Ogre::SceneNode* node, Ogre::SceneNode* parent, bool visible);

View File

@ -84,6 +84,11 @@ const std::string &DisplaySolution::name(const IndexPair &idx_pair) const
return data_[idx_pair.first].name_; return data_[idx_pair.first].name_;
} }
uint32_t DisplaySolution::creatorId(const DisplaySolution::IndexPair &idx_pair) const
{
return data_[idx_pair.first].creator_id_;
}
const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::IndexPair &idx_pair) const const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::IndexPair &idx_pair) const
{ {
return data_[idx_pair.first].markers_; return data_[idx_pair.first].markers_;
@ -112,6 +117,7 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta
data_[i].trajectory_.reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), "")); data_[i].trajectory_.reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), ""));
data_[i].trajectory_->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory); data_[i].trajectory_->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory);
data_[i].name_ = sub.name; data_[i].name_ = sub.name;
data_[i].creator_id_ = sub.stage_id;
steps_ += data_[i].trajectory_->getWayPointCount(); steps_ += data_[i].trajectory_->getWayPointCount();
ref_scene->setPlanningSceneDiffMsg(sub.scene_diff); ref_scene->setPlanningSceneDiffMsg(sub.scene_diff);

View File

@ -446,6 +446,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
moveit::core::RobotStateConstPtr robot_state; moveit::core::RobotStateConstPtr robot_state;
planning_scene::PlanningSceneConstPtr scene; planning_scene::PlanningSceneConstPtr scene;
if (index == displaying_solution_->getWayPointCount()) { if (index == displaying_solution_->getWayPointCount()) {
// render last state
scene = displaying_solution_->scene(index); scene = displaying_solution_->scene(index);
renderPlanningScene (scene); renderPlanningScene (scene);
robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); robot_state.reset(new moveit::core::RobotState(scene->getCurrentState()));
@ -454,8 +455,11 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
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) {
// switch to new stage: show new planning scene
renderPlanningScene (scene); renderPlanningScene (scene);
Q_EMIT activeStageChanged(displaying_solution_->creatorId(idx_pair));
}
robot_state = displaying_solution_->getWayPointPtr(idx_pair); robot_state = displaying_solution_->getWayPointPtr(idx_pair);
} }