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;
}
QModelIndex LocalTaskModel::indexFromStageId(size_t id) const
{
// TODO implement
return QModelIndex();
}
QAbstractItemModel *LocalTaskModel::getSolutionModel(const QModelIndex &index)
{
// TODO implement

View File

@ -72,6 +72,8 @@ public:
void setStageFactory(const StageFactoryPtr &factory) 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;
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)
return false;
TreeMergeProxyModel::removeRows(row, count, parent);
return TreeMergeProxyModel::removeRows(row, count, parent);
}
std::pair<TaskListModel*, TaskDisplay*>

View File

@ -220,6 +220,12 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i
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)
{
// 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;
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 processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg);

View File

@ -83,6 +83,8 @@ TaskDisplay::TaskDisplay() : Display()
this, SLOT(changedTaskSolutionTopic()), 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);

View File

@ -212,6 +212,23 @@ Qt::ItemFlags TaskListModel::flags(const QModelIndex &index) const
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
{
if (orientation == Qt::Horizontal)
@ -220,6 +237,15 @@ QVariant TaskListModel::headerData(int section, Qt::Orientation orientation, int
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:
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
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
remote_task = new RemoteTaskModel(scene_, this);
remote_task->setSolutionClient(get_solution_client_);
// HACK: always use the last created model as active
active_task_model_ = remote_task;
}
if (!remote_task)
return; // task is not in use anymore

View File

@ -48,6 +48,7 @@
#include <QAbstractItemModel>
#include <QTreeView>
#include <memory>
#include <QPointer>
namespace ros { class ServiceClient; }
namespace rviz { class PropertyTreeModel; }
@ -86,6 +87,9 @@ public:
unsigned int taskFlags() const { return flags_; }
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
virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0;
/// get solution for given solution index
@ -119,6 +123,9 @@ class TaskListModel : public utils::FlatMergeProxyModel {
// factory used to create stages
StageFactoryPtr stage_factory_;
QPointer<BaseTaskModel> active_task_model_;
QPersistentModelIndex highlighted_row_index_;
void onRemoveModel(QAbstractItemModel *model) override;
public:
@ -127,10 +134,12 @@ public:
void setScene(const planning_scene::PlanningSceneConstPtr& scene);
void setSolutionClient(ros::ServiceClient* client);
void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; }
int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; }
static QVariant horizontalHeader(int column, int role);
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
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;
Qt::DropActions supportedDropActions() 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);
}
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
{
if (!index.isValid())

View File

@ -75,6 +75,9 @@ public:
QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) 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;
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const 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_;
/// optional name of the trajectory
std::string name_;
/// id of creating stage
uint32_t creator_id_;
/// rviz markers
MarkerVisualizationPtr markers_;
};
@ -91,6 +93,7 @@ public:
size_t getWayPointCount() const { return steps_; }
bool empty() const { return steps_ == 0; }
/// pair of trajectory part and way point index within part
typedef std::pair<size_t, size_t> IndexPair;
IndexPair indexPair(size_t index) const;
@ -113,6 +116,8 @@ public:
const std::string& name(size_t index) const {
return name(indexPair(index));
}
uint32_t creatorId(const IndexPair& idx_pair) const;
const MarkerVisualizationPtr markers(const IndexPair& idx_pair) const;
const MarkerVisualizationPtr markers(size_t index) const {
return markers(indexPair(index));

View File

@ -127,6 +127,9 @@ private Q_SLOTS:
void changedSceneEnabled();
void renderCurrentScene();
Q_SIGNALS:
void activeStageChanged(size_t);
protected:
void setVisibility(); // set visibility of main scene node
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_;
}
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
{
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_->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory);
data_[i].name_ = sub.name;
data_[i].creator_id_ = sub.stage_id;
steps_ += data_[i].trajectory_->getWayPointCount();
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;
planning_scene::PlanningSceneConstPtr scene;
if (index == displaying_solution_->getWayPointCount()) {
// render last state
scene = displaying_solution_->scene(index);
renderPlanningScene (scene);
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);
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);
Q_EMIT activeStageChanged(displaying_solution_->creatorId(idx_pair));
}
robot_state = displaying_solution_->getWayPointPtr(idx_pair);
}