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;
|
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
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
|
|||||||
@ -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*>
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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);
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -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())
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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));
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user