mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
highlight currently active solution part
This commit is contained in:
parent
0fd5f3c1b3
commit
3d07e34522
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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*>
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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())
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user