diff --git a/visualization/motion_planning_tasks/task_display.cpp b/visualization/motion_planning_tasks/task_display.cpp index 460440a0..9fb0c7d4 100644 --- a/visualization/motion_planning_tasks/task_display.cpp +++ b/visualization/motion_planning_tasks/task_display.cpp @@ -71,6 +71,8 @@ TaskDisplay::TaskDisplay() : Display() trajectory_visual_.reset(new TaskSolutionVisualization(this, this)); + tasks_property_ = + new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this); } void TaskDisplay::onInitialize() @@ -149,11 +151,17 @@ void TaskDisplay::changedRobotDescription() void TaskDisplay::updateTaskListModel() { - task_model_ = TaskListModelCache::instance().getModel( + if (task_list_model_) { + disconnect(task_list_model_.get(), &TaskListModel::rowsInserted, this, &TaskDisplay::onTasksInserted); + disconnect(task_list_model_.get(), &TaskListModel::rowsAboutToBeRemoved, this, &TaskDisplay::onTasksRemoved); + } + tasks_property_->removeChildren(); + + task_list_model_ = TaskListModelCache::instance().getModel( task_monitor_topic_property_->getString(), task_solution_topic_property_->getString()); - if (!task_model_) { + if (!task_list_model_) { if (task_monitor_topic_property_->getString().isEmpty()) setStatus(rviz::StatusProperty::Warn, "Task Monitor", "invalid task monitor topic"); else if (task_solution_topic_property_->getString().isEmpty()) @@ -163,19 +171,25 @@ void TaskDisplay::updateTaskListModel() } else { boost::function taskCB ([this](const moveit_task_constructor::TaskConstPtr &msg){ - mainloop_jobs_.addJob([this, msg]() { task_model_->processTaskMessage(*msg); }); + mainloop_jobs_.addJob([this, msg]() { task_list_model_->processTaskMessage(*msg); }); }); task_monitor_sub = update_nh_.subscribe(task_monitor_topic_property_->getStdString(), 2, taskCB); boost::function solCB ([this](const moveit_task_constructor::SolutionConstPtr &msg){ - mainloop_jobs_.addJob([this, msg]() { task_model_->processSolutionMessage(*msg); }); + mainloop_jobs_.addJob([this, msg]() { task_list_model_->processSolutionMessage(*msg); }); // TODO: use already processed trajectory (e.g. by ID) - mainloop_jobs_.addJob([this, msg]() {trajectory_visual_->showTrajectory(*msg);}); + mainloop_jobs_.addJob([this, msg]() { + trajectory_visual_->showTrajectory(*msg); + }); }); task_solution_sub = update_nh_.subscribe(task_solution_topic_property_->getStdString(), 2, solCB); setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected"); + + onTasksInserted(QModelIndex(), 0, task_list_model_->rowCount()-1); + connect(task_list_model_.get(), &TaskListModel::rowsInserted, this, &TaskDisplay::onTasksInserted); + connect(task_list_model_.get(), &TaskListModel::rowsAboutToBeRemoved, this, &TaskDisplay::onTasksRemoved); } } @@ -191,4 +205,25 @@ void TaskDisplay::changedTaskSolutionTopic() updateTaskListModel(); } +void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last) +{ + if (parent.isValid()) return; + + TaskListModel* m = static_cast(sender()); + for (; first <= last; ++first) { + QModelIndex idx = m->index(first, 0, parent); + tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data())); + } +} + +void TaskDisplay::onTasksRemoved(const QModelIndex &parent, int first, int last) +{ + if (parent.isValid()) return; + + for (; first <= last; ++first) { + rviz::Property *child = tasks_property_->takeChildAt(first); + delete child; + } +} + } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/task_display.h b/visualization/motion_planning_tasks/task_display.h index 86fc52fe..1a7756b8 100644 --- a/visualization/motion_planning_tasks/task_display.h +++ b/visualization/motion_planning_tasks/task_display.h @@ -86,6 +86,8 @@ private Q_SLOTS: void changedRobotDescription(); void changedTaskMonitorTopic(); void changedTaskSolutionTopic(); + void onTasksInserted(const QModelIndex& parent, int first, int last); + void onTasksRemoved(const QModelIndex& parent, int first, int last); protected: void updateTaskListModel(); @@ -99,7 +101,7 @@ protected: // The trajectory playback component TaskSolutionVisualizationPtr trajectory_visual_; // The TaskListModel storing actual task and solution data - TaskListModelPtr task_model_; + TaskListModelPtr task_list_model_; // Load robot model rdf_loader::RDFLoaderPtr rdf_loader_; @@ -109,6 +111,7 @@ protected: rviz::StringProperty* robot_description_property_; rviz::RosTopicProperty* task_monitor_topic_property_; rviz::RosTopicProperty* task_solution_topic_property_; + rviz::Property* tasks_property_; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/task_panel.cpp b/visualization/motion_planning_tasks/task_panel.cpp index fcd37116..2be19fe9 100644 --- a/visualization/motion_planning_tasks/task_panel.cpp +++ b/visualization/motion_planning_tasks/task_panel.cpp @@ -93,7 +93,7 @@ TaskPanel::~TaskPanel() TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr) : q_ptr(q_ptr) , task_list_model(TaskListModelCache::instance().getGlobalModel()) - , settings(new rviz::PropertyTreeModel(new rviz::Property)) + , settings(new rviz::PropertyTreeModel(new rviz::Property, q_ptr)) { setupUi(q_ptr); initSettings(settings->getRoot());