diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index b756d1c6..5563739c 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -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 diff --git a/visualization/motion_planning_tasks/src/local_task_model.h b/visualization/motion_planning_tasks/src/local_task_model.h index b9873a80..dbedd86f 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -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; diff --git a/visualization/motion_planning_tasks/src/meta_task_list_model.cpp b/visualization/motion_planning_tasks/src/meta_task_list_model.cpp index 582f6bcf..3584ee52 100644 --- a/visualization/motion_planning_tasks/src/meta_task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/meta_task_list_model.cpp @@ -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 diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index b6e07f98..666a6114 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -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 diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index dd770cec..2a07dec4 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -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); diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index ced3f388..92da2f82 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -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); diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index f95268d4..e8ac2a52 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -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 diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 753e5f4a..3a51b40c 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -48,6 +48,7 @@ #include #include #include +#include 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 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); }; diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp index 614b6fb2..b6d818ee 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp @@ -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()) diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h index 3c5f7ab5..4fd5b2e4 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h @@ -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; diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 17d2c50f..2970a130 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -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 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)); diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index c3a3718a..f4f70cd1 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -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); diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index d0280884..7b214c7c 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -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); diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index f2aebdbd..5560ea7a 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -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); }