TaskDisplay: show tasks

This commit is contained in:
Robert Haschke 2017-11-12 05:29:07 +01:00
parent 924051c511
commit 2bc1b08a00
3 changed files with 45 additions and 7 deletions

View File

@ -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<void(const moveit_task_constructor::TaskConstPtr &)> 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<void(const moveit_task_constructor::SolutionConstPtr &)> 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<TaskListModel*>(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

View File

@ -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

View File

@ -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());