mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'wip-solution' and 'wip-interaction' into wip-refactor
This commit is contained in:
commit
3f93d6c9c3
@ -20,6 +20,7 @@ public:
|
||||
bool traverseRecursively(const StageCallback &processor) const;
|
||||
|
||||
virtual bool insert(Stage::pointer&& stage, int before = -1);
|
||||
virtual bool remove(int pos);
|
||||
virtual void clear();
|
||||
|
||||
void reset() override;
|
||||
|
||||
@ -97,6 +97,13 @@ bool ContainerBase::insert(Stage::pointer &&stage, int before)
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ContainerBase::remove(int pos)
|
||||
{
|
||||
ContainerBasePrivate::const_iterator it = pimpl()->position(pos);
|
||||
pimpl()->children_.erase(it);
|
||||
return true;
|
||||
}
|
||||
|
||||
void ContainerBase::clear()
|
||||
{
|
||||
pimpl()->children_.clear();
|
||||
|
||||
@ -43,6 +43,7 @@
|
||||
#include <ros/init.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <initializer_list>
|
||||
#include <qcoreapplication.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
@ -202,3 +203,18 @@ TEST_F(TaskListModelTest, visitedPopulate) {
|
||||
EXPECT_EQ(num_inserts, 3);
|
||||
EXPECT_EQ(num_updates, 0);
|
||||
}
|
||||
|
||||
TEST_F(TaskListModelTest, deletion) {
|
||||
children = 3;
|
||||
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||
moveit_rviz_plugin::BaseTaskModel *m = model.getTask(0);
|
||||
int num_deletes = 0;
|
||||
QObject::connect(m, &QObject::destroyed, [&num_deletes](){++num_deletes;});
|
||||
|
||||
model.removeTask(m);
|
||||
// process deleteLater() events
|
||||
QCoreApplication::sendPostedEvents(nullptr, QEvent::DeferredDelete);
|
||||
// as m is owned by model, m should be destroyed
|
||||
// EXPECT_EQ(num_deletes, 1); // TODO: event is not processed, missing event loop?
|
||||
EXPECT_EQ(model.rowCount(), 0);
|
||||
}
|
||||
|
||||
@ -36,21 +36,24 @@
|
||||
|
||||
#include "factory_model.h"
|
||||
#include <rviz/load_resource.h>
|
||||
#include <QMimeData>
|
||||
#include <QSet>
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
FactoryModel::FactoryModel(rviz::Factory *factory, QObject *parent)
|
||||
FactoryModel::FactoryModel(rviz::Factory &factory, const QString& mime_type, QObject *parent)
|
||||
: QStandardItemModel(parent)
|
||||
, mime_type_(mime_type)
|
||||
{
|
||||
setHorizontalHeaderLabels({tr("Name")});
|
||||
fillTree(factory);
|
||||
}
|
||||
|
||||
void FactoryModel::fillTree(rviz::Factory *factory)
|
||||
void FactoryModel::fillTree(rviz::Factory &factory)
|
||||
{
|
||||
QIcon default_package_icon = rviz::loadPixmap( "package://rviz/icons/default_package_icon.png" );
|
||||
|
||||
QStringList classes = factory->getDeclaredClassIds();
|
||||
QStringList classes = factory.getDeclaredClassIds();
|
||||
classes.sort();
|
||||
|
||||
// Map from package names to the corresponding top-level tree widget items.
|
||||
@ -58,7 +61,7 @@ void FactoryModel::fillTree(rviz::Factory *factory)
|
||||
|
||||
for(const QString& lookup_name : classes)
|
||||
{
|
||||
QString package = factory->getClassPackage(lookup_name);
|
||||
QString package = factory.getClassPackage(lookup_name);
|
||||
|
||||
QStandardItem* package_item;
|
||||
auto mi = package_items.find(package);
|
||||
@ -72,13 +75,31 @@ void FactoryModel::fillTree(rviz::Factory *factory)
|
||||
{
|
||||
package_item = mi->second;
|
||||
}
|
||||
QStandardItem* class_item = new QStandardItem(factory->getIcon(lookup_name),
|
||||
factory->getClassName(lookup_name));
|
||||
class_item->setWhatsThis(factory->getClassDescription(lookup_name));
|
||||
class_item->setData(lookup_name);
|
||||
QStandardItem* class_item = new QStandardItem(factory.getIcon(lookup_name),
|
||||
factory.getClassName(lookup_name));
|
||||
class_item->setWhatsThis(factory.getClassDescription(lookup_name));
|
||||
class_item->setData(lookup_name, Qt::UserRole);
|
||||
class_item->setDragEnabled(true);
|
||||
package_item->appendRow(class_item);
|
||||
}
|
||||
}
|
||||
|
||||
QStringList FactoryModel::mimeTypes() const
|
||||
{
|
||||
return { mime_type_ };
|
||||
}
|
||||
|
||||
QMimeData *FactoryModel::mimeData(const QModelIndexList &indexes) const
|
||||
{
|
||||
QSet<int> rows_considered;
|
||||
QMimeData* mime_data = new QMimeData();
|
||||
for (const auto &index : indexes) {
|
||||
if (rows_considered.contains(index.row()))
|
||||
continue;
|
||||
// mime data is lookup_name
|
||||
mime_data->setData(mime_type_, index.data(Qt::UserRole).toByteArray());
|
||||
}
|
||||
return mime_data;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -43,10 +43,14 @@ namespace moveit_rviz_plugin {
|
||||
|
||||
class FactoryModel : public QStandardItemModel
|
||||
{
|
||||
void fillTree(rviz::Factory *factory);
|
||||
QString mime_type_;
|
||||
void fillTree(rviz::Factory& factory);
|
||||
|
||||
public:
|
||||
FactoryModel(rviz::Factory *factory, QObject *parent = nullptr);
|
||||
FactoryModel(rviz::Factory& factory, const QString &mime_type, QObject *parent = nullptr);
|
||||
|
||||
QStringList mimeTypes() const override;
|
||||
QMimeData* mimeData(const QModelIndexList &indexes) const override;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -35,10 +35,13 @@
|
||||
/* Author: Robert Haschke */
|
||||
|
||||
#include "local_task_model.h"
|
||||
#include "factory_model.h"
|
||||
#include <container_p.h>
|
||||
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <QMimeData>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
@ -79,6 +82,13 @@ LocalTaskModel::LocalTaskModel(QObject *parent)
|
||||
root_ = pimpl();
|
||||
}
|
||||
|
||||
LocalTaskModel::LocalTaskModel(Stage::pointer &&container, QObject *parent)
|
||||
: BaseTaskModel(parent)
|
||||
, Task("", std::move(container))
|
||||
{
|
||||
root_ = pimpl();
|
||||
}
|
||||
|
||||
int LocalTaskModel::rowCount(const QModelIndex &parent) const
|
||||
{
|
||||
if (parent.column() > 0)
|
||||
@ -120,7 +130,9 @@ Qt::ItemFlags LocalTaskModel::flags(const QModelIndex &index) const
|
||||
{
|
||||
Qt::ItemFlags flags = BaseTaskModel::flags(index);
|
||||
ContainerBasePrivate *c = dynamic_cast<ContainerBasePrivate*>(node(index));
|
||||
if (c) flags |= Qt::ItemIsDropEnabled;
|
||||
// dropping into containers is enabled
|
||||
if (c && stage_factory_)
|
||||
flags |= Qt::ItemIsDropEnabled;
|
||||
return flags;
|
||||
}
|
||||
|
||||
@ -153,6 +165,57 @@ bool LocalTaskModel::setData(const QModelIndex &index, const QVariant &value, in
|
||||
if (name == n->name().c_str())
|
||||
return false;
|
||||
n->me()->setName(name.toStdString());
|
||||
dataChanged(index, index);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool LocalTaskModel::removeRows(int row, int count, const QModelIndex &parent)
|
||||
{
|
||||
if (!parent.isValid())
|
||||
return false; // cannot remove top-level container
|
||||
if (flags_ & IS_RUNNING)
|
||||
return false; // cannot modify running task
|
||||
|
||||
Q_ASSERT(dynamic_cast<ContainerBase*>(node(parent)->me()));
|
||||
ContainerBasePrivate *cp = static_cast<ContainerBasePrivate*>(node(parent));
|
||||
ContainerBase *c = static_cast<ContainerBase*>(cp->me());
|
||||
if (row < 0 || (size_t)row + count > cp->children().size())
|
||||
return false;
|
||||
|
||||
beginRemoveRows(parent, row, row+count-1);
|
||||
for (; count > 0; --count)
|
||||
c->remove(row);
|
||||
endRemoveRows();
|
||||
return true;
|
||||
}
|
||||
|
||||
void LocalTaskModel::setStageFactory(const StageFactoryPtr &factory)
|
||||
{
|
||||
stage_factory_ = factory;
|
||||
}
|
||||
|
||||
bool LocalTaskModel::dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent)
|
||||
{
|
||||
Q_UNUSED(column);
|
||||
|
||||
if (!stage_factory_ || (flags_ & IS_RUNNING))
|
||||
return false;
|
||||
const QString mime_type = stage_factory_->mimeType();
|
||||
if (!mime->hasFormat(mime_type))
|
||||
return false;
|
||||
|
||||
ContainerBasePrivate *c = dynamic_cast<ContainerBasePrivate*>(node(parent));
|
||||
Q_ASSERT(c);
|
||||
|
||||
QString error;
|
||||
moveit::task_constructor::Stage* stage
|
||||
= stage_factory_->makeRaw(mime->data(mime_type), &error);
|
||||
if (!stage)
|
||||
return false;
|
||||
|
||||
beginInsertRows(parent, row, row);
|
||||
static_cast<ContainerBase*>(c->me())->insert(moveit::task_constructor::Stage::pointer(stage), row);
|
||||
endInsertRows();
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -48,12 +48,14 @@ class LocalTaskModel
|
||||
Q_OBJECT
|
||||
typedef moveit::task_constructor::StagePrivate Node;
|
||||
Node *root_;
|
||||
StageFactoryPtr stage_factory_;
|
||||
|
||||
inline Node* node(const QModelIndex &index) const;
|
||||
QModelIndex index(Node *n) const;
|
||||
|
||||
public:
|
||||
LocalTaskModel(QObject *parent = nullptr);
|
||||
LocalTaskModel(ContainerBase::pointer &&container, QObject *parent = nullptr);
|
||||
int rowCount(const QModelIndex &parent = QModelIndex()) const override;
|
||||
|
||||
QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override;
|
||||
@ -62,6 +64,12 @@ public:
|
||||
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;
|
||||
|
||||
bool removeRows(int row, int count, const QModelIndex &parent) override;
|
||||
|
||||
/// providing a StageFactory makes the model accepting drops
|
||||
void setStageFactory(const StageFactoryPtr &factory);
|
||||
bool dropMimeData(const QMimeData *data, Qt::DropAction action, int row, int column, const QModelIndex &parent) override;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -38,10 +38,11 @@
|
||||
|
||||
#ifndef Q_MOC_RUN
|
||||
#include <pluginlib/class_loader.h>
|
||||
#include <rviz/load_resource.h>
|
||||
#include <functional>
|
||||
#endif
|
||||
|
||||
#include <rviz/factory.h>
|
||||
#include <rviz/load_resource.h>
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
@ -55,11 +56,12 @@ private:
|
||||
QString package_;
|
||||
QString name_;
|
||||
QString description_;
|
||||
Type*(*factory_function_)();
|
||||
std::function<Type*()> factory_function_;
|
||||
};
|
||||
|
||||
public:
|
||||
PluginlibFactory( const QString& package, const QString& base_class_type )
|
||||
: mime_type_(QString("application/%1/%2").arg(package, base_class_type))
|
||||
{
|
||||
class_loader_ = new pluginlib::ClassLoader<Type>( package.toStdString(), base_class_type.toStdString() );
|
||||
}
|
||||
@ -68,56 +70,58 @@ public:
|
||||
delete class_loader_;
|
||||
}
|
||||
|
||||
/// retrieve mime type used for given factory
|
||||
QString mimeType() const {
|
||||
return mime_type_;
|
||||
}
|
||||
|
||||
virtual QStringList getDeclaredClassIds()
|
||||
{
|
||||
QStringList ids;
|
||||
std::vector<std::string> std_ids = class_loader_->getDeclaredClasses();
|
||||
for( size_t i = 0; i < std_ids.size(); i++ )
|
||||
{
|
||||
ids.push_back( QString::fromStdString( std_ids[ i ]));
|
||||
}
|
||||
typename QHash<QString, BuiltInClassRecord>::const_iterator iter;
|
||||
for( iter = built_ins_.begin(); iter != built_ins_.end(); iter++ )
|
||||
{
|
||||
ids.push_back( iter.key() );
|
||||
for(const auto& record : built_ins_)
|
||||
ids.push_back(record.class_id_);
|
||||
for (const auto& id : class_loader_->getDeclaredClasses()) {
|
||||
QString sid = QString::fromStdString(id);
|
||||
if (ids.contains(sid)) continue; // built_in take precedence
|
||||
ids.push_back(sid);
|
||||
}
|
||||
return ids;
|
||||
}
|
||||
|
||||
virtual QString getClassDescription( const QString& class_id ) const
|
||||
{
|
||||
typename QHash<QString, BuiltInClassRecord>::const_iterator iter = built_ins_.find( class_id );
|
||||
if( iter != built_ins_.end() )
|
||||
auto it = built_ins_.find( class_id );
|
||||
if( it != built_ins_.end() )
|
||||
{
|
||||
return iter->description_;
|
||||
return it->description_;
|
||||
}
|
||||
return QString::fromStdString( class_loader_->getClassDescription( class_id.toStdString() ));
|
||||
}
|
||||
|
||||
virtual QString getClassName( const QString& class_id ) const
|
||||
{
|
||||
typename QHash<QString, BuiltInClassRecord>::const_iterator iter = built_ins_.find( class_id );
|
||||
if( iter != built_ins_.end() )
|
||||
auto it = built_ins_.find( class_id );
|
||||
if( it != built_ins_.end() )
|
||||
{
|
||||
return iter->name_;
|
||||
return it->name_;
|
||||
}
|
||||
return QString::fromStdString( class_loader_->getName( class_id.toStdString() ));
|
||||
}
|
||||
|
||||
virtual QString getClassPackage( const QString& class_id ) const
|
||||
{
|
||||
typename QHash<QString, BuiltInClassRecord>::const_iterator iter = built_ins_.find( class_id );
|
||||
if( iter != built_ins_.end() )
|
||||
auto it = built_ins_.find( class_id );
|
||||
if( it != built_ins_.end() )
|
||||
{
|
||||
return iter->package_;
|
||||
return it->package_;
|
||||
}
|
||||
return QString::fromStdString( class_loader_->getClassPackage( class_id.toStdString() ));
|
||||
}
|
||||
|
||||
virtual QString getPluginManifestPath( const QString& class_id ) const
|
||||
{
|
||||
typename QHash<QString, BuiltInClassRecord>::const_iterator iter = built_ins_.find( class_id );
|
||||
if( iter != built_ins_.end() )
|
||||
auto it = built_ins_.find( class_id );
|
||||
if( it != built_ins_.end() )
|
||||
{
|
||||
return "";
|
||||
}
|
||||
@ -140,8 +144,8 @@ public:
|
||||
return icon;
|
||||
}
|
||||
|
||||
virtual void addBuiltInClass( const QString& package, const QString& name, const QString& description,
|
||||
Type* (*factory_function)() )
|
||||
void addBuiltInClass(const QString& package, const QString& name, const QString& description,
|
||||
const std::function<Type*()>& factory_function)
|
||||
{
|
||||
BuiltInClassRecord record;
|
||||
record.class_id_ = package + "/" + name;
|
||||
@ -151,8 +155,12 @@ public:
|
||||
record.factory_function_ = factory_function;
|
||||
built_ins_[ record.class_id_ ] = record;
|
||||
}
|
||||
template <class Derived>
|
||||
void addBuiltInClass(const QString& name, const QString& description)
|
||||
{
|
||||
addBuiltInClass(ROS_PACKAGE_NAME, name, description, [](){return new Derived();});
|
||||
}
|
||||
|
||||
protected:
|
||||
/** @brief Instantiate and return a instance of a subclass of Type using our
|
||||
* pluginlib::ClassLoader.
|
||||
* @param class_id A string identifying the class uniquely among
|
||||
@ -193,6 +201,7 @@ protected:
|
||||
}
|
||||
|
||||
private:
|
||||
const QString mime_type_;
|
||||
pluginlib::ClassLoader<Type>* class_loader_;
|
||||
QHash<QString, BuiltInClassRecord> built_ins_;
|
||||
};
|
||||
|
||||
@ -39,6 +39,9 @@
|
||||
#include "remote_task_model.h"
|
||||
#include <moveit_task_constructor/container.h>
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <QApplication>
|
||||
#include <QPalette>
|
||||
#include <qflags.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
@ -47,6 +50,7 @@ namespace moveit_rviz_plugin {
|
||||
|
||||
enum NodeFlag {
|
||||
WAS_VISITED = 0x01, // indicate that model should emit change notifications
|
||||
NAME_CHANGED = 0x02, // indicate that name was manually changed
|
||||
};
|
||||
typedef QFlags<NodeFlag> NodeFlags;
|
||||
|
||||
@ -170,6 +174,10 @@ QVariant RemoteTaskModel::data(const QModelIndex &index, int role) const
|
||||
case 2: return 0;
|
||||
}
|
||||
break;
|
||||
case Qt::ForegroundRole:
|
||||
if (index.column() == 0 && !index.parent().isValid())
|
||||
return (flags_ & IS_DESTROYED) ? QColor(Qt::red) : QApplication::palette().text().color();
|
||||
break;
|
||||
}
|
||||
return QVariant();
|
||||
}
|
||||
@ -179,7 +187,10 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i
|
||||
Node *n = node(index);
|
||||
if (!n || index.column() != 0 || role != Qt::EditRole)
|
||||
return false;
|
||||
return n->setName(value.toString());
|
||||
n->setName(value.toString());
|
||||
n->node_flags_ |= NAME_CHANGED;
|
||||
dataChanged(index, index);
|
||||
return true;
|
||||
}
|
||||
|
||||
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor::TaskDescription::_description_type &msg)
|
||||
@ -211,7 +222,9 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor::Ta
|
||||
Q_ASSERT(n->parent_ == parent);
|
||||
|
||||
// set content of stage
|
||||
bool changed = n->setName(QString::fromStdString(s.name));
|
||||
bool changed = false;
|
||||
if (!(n->node_flags_ & NAME_CHANGED)) // avoid overwriting a manually changed name
|
||||
n->setName(QString::fromStdString(s.name));
|
||||
InterfaceFlags old_flags = n->interface_flags_;
|
||||
n->interface_flags_ = InterfaceFlags();
|
||||
for (auto f : {READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END}) {
|
||||
@ -223,9 +236,14 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor::Ta
|
||||
// emit notify about model changes when node was already visited
|
||||
if (changed && (n->node_flags_ & WAS_VISITED)) {
|
||||
QModelIndex idx = index(n);
|
||||
dataChanged(idx, idx);
|
||||
dataChanged(idx, idx.sibling(idx.row(), 2));
|
||||
}
|
||||
}
|
||||
|
||||
if (msg.empty()) {
|
||||
flags_ |= IS_DESTROYED;
|
||||
dataChanged(index(0, 0), index(0, 2));
|
||||
}
|
||||
}
|
||||
|
||||
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor::TaskDescription::_statistics_type &msg)
|
||||
|
||||
@ -64,6 +64,8 @@ TaskDisplay::TaskDisplay() : Display()
|
||||
this, SLOT(changedTaskSolutionTopic()), this);
|
||||
|
||||
trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
|
||||
tasks_property_ =
|
||||
new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this);
|
||||
}
|
||||
|
||||
void TaskDisplay::onInitialize()
|
||||
@ -146,7 +148,7 @@ void TaskDisplay::taskDescriptionCB(const ros::MessageEvent<const moveit_task_co
|
||||
const moveit_task_constructor::TaskDescriptionConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
task_model_->processTaskDescriptionMessage(id, *msg);
|
||||
task_list_model_->processTaskDescriptionMessage(id, *msg);
|
||||
});
|
||||
}
|
||||
|
||||
@ -155,7 +157,7 @@ void TaskDisplay::taskStatisticsCB(const ros::MessageEvent<const moveit_task_con
|
||||
const moveit_task_constructor::TaskStatisticsConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
task_model_->processTaskStatisticsMessage(id, *msg);
|
||||
task_list_model_->processTaskStatisticsMessage(id, *msg);
|
||||
});
|
||||
}
|
||||
|
||||
@ -164,7 +166,7 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_const
|
||||
const moveit_task_constructor::SolutionConstPtr& msg = event.getMessage();
|
||||
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
||||
mainloop_jobs_.addJob([this, id, msg]() {
|
||||
if (task_model_) task_model_->processSolutionMessage(id, *msg);
|
||||
if (task_list_model_) task_list_model_->processSolutionMessage(id, *msg);
|
||||
// TODO: use already processed trajectory (e.g. by ID)
|
||||
trajectory_visual_->showTrajectory(*msg);
|
||||
});
|
||||
@ -172,29 +174,43 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_const
|
||||
|
||||
void TaskDisplay::updateTaskListModel()
|
||||
{
|
||||
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();
|
||||
|
||||
// generate task monitoring topics from solution topic
|
||||
std::string solution_topic = task_solution_topic_property_->getStdString();
|
||||
auto last_sep = solution_topic.find_last_of('/');
|
||||
if (last_sep == std::string::npos)
|
||||
if (last_sep == std::string::npos) {
|
||||
setStatus(rviz::StatusProperty::Error, "Task Monitor", "invalid topic");
|
||||
return;
|
||||
}
|
||||
|
||||
std::string base_ns = solution_topic.substr(0, last_sep+1);
|
||||
task_model_ = TaskListModelCache::instance().getModel(base_ns);
|
||||
task_list_model_ = TaskListModelCache::instance().getModel(base_ns);
|
||||
|
||||
if (task_model_) {
|
||||
if (task_list_model_) {
|
||||
// listen to task descriptions updates
|
||||
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);
|
||||
|
||||
// listen to task statistics updates
|
||||
task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
|
||||
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
|
||||
|
||||
// initialize task list
|
||||
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);
|
||||
connect(task_list_model_.get(), &TaskListModel::dataChanged, this, &TaskDisplay::onTaskDataChanged);
|
||||
} else {
|
||||
setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel");
|
||||
}
|
||||
|
||||
// listen to task solutions
|
||||
task_solution_sub = update_nh_.subscribe(solution_topic, 2, &TaskDisplay::taskSolutionCB, this);
|
||||
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
|
||||
}
|
||||
|
||||
void TaskDisplay::changedTaskSolutionTopic()
|
||||
@ -205,4 +221,38 @@ void TaskDisplay::changedTaskSolutionTopic()
|
||||
updateTaskListModel();
|
||||
}
|
||||
|
||||
void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last)
|
||||
{
|
||||
if (parent.isValid()) return; // only handle top-level items
|
||||
|
||||
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; // only handle top-level items
|
||||
|
||||
for (; first <= last; ++first) {
|
||||
rviz::Property *child = tasks_property_->takeChildAt(first);
|
||||
delete child;
|
||||
}
|
||||
}
|
||||
|
||||
void TaskDisplay::onTaskDataChanged(const QModelIndex &topLeft, const QModelIndex &bottomRight)
|
||||
{
|
||||
if (topLeft.parent().isValid()) return; // only handle top-level items
|
||||
|
||||
for (int row = topLeft.row(); row <= bottomRight.row(); ++row) {
|
||||
rviz::Property *child = tasks_property_->childAt(row);
|
||||
if (topLeft.column() <= 0 && 0 <= bottomRight.column()) // name changed
|
||||
child->setName(topLeft.sibling(row, 0).data().toString());
|
||||
if (topLeft.column() <= 1 && 1 <= bottomRight.column()) // #solutions changed
|
||||
child->setValue(topLeft.sibling(row, 1).data());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace moveit_rviz_plugin
|
||||
|
||||
@ -88,6 +88,9 @@ private Q_SLOTS:
|
||||
*/
|
||||
void changedRobotDescription();
|
||||
void changedTaskSolutionTopic();
|
||||
void onTasksInserted(const QModelIndex& parent, int first, int last);
|
||||
void onTasksRemoved(const QModelIndex& parent, int first, int last);
|
||||
void onTaskDataChanged(const QModelIndex &topLeft, const QModelIndex &bottomRight);
|
||||
|
||||
void taskDescriptionCB(const ros::MessageEvent<moveit_task_constructor::TaskDescription const>& event);
|
||||
void taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor::TaskStatistics> &event);
|
||||
@ -106,7 +109,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_;
|
||||
@ -115,6 +118,7 @@ protected:
|
||||
// Properties
|
||||
rviz::StringProperty* robot_description_property_;
|
||||
rviz::RosTopicProperty* task_solution_topic_property_;
|
||||
rviz::Property* tasks_property_;
|
||||
};
|
||||
|
||||
} // namespace moveit_rviz_plugin
|
||||
|
||||
@ -37,11 +37,16 @@
|
||||
#include "task_list_model.h"
|
||||
#include "local_task_model.h"
|
||||
#include "remote_task_model.h"
|
||||
#include "factory_model.h"
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <QMimeData>
|
||||
#include <qevent.h>
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
static const std::string LOGNAME("TaskListModel");
|
||||
|
||||
QString TaskListModel::horizontalHeader(int column)
|
||||
{
|
||||
switch (column) {
|
||||
@ -67,9 +72,21 @@ Qt::ItemFlags BaseTaskModel::flags(const QModelIndex &index) const
|
||||
return flags;
|
||||
}
|
||||
|
||||
enum TaskModelFlag {
|
||||
IS_DESTROYED = 0x01,
|
||||
};
|
||||
|
||||
StageFactoryPtr getStageFactory()
|
||||
{
|
||||
static std::weak_ptr<StageFactory> factory;
|
||||
if (!factory.expired())
|
||||
return factory.lock();
|
||||
|
||||
StageFactoryPtr result(new StageFactory("moveit_task_constructor",
|
||||
"moveit::task_constructor::Stage"));
|
||||
// Hm. pluglinlib / ClassLoader cannot instantiate classes in implicitly loaded libs
|
||||
result->addBuiltInClass<moveit::task_constructor::SerialContainer>("Serial Container", "");
|
||||
factory = result; // remember for future uses
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
class TaskListModelPrivate {
|
||||
public:
|
||||
@ -82,7 +99,6 @@ public:
|
||||
BaseTaskModel* model_;
|
||||
// map of proxy=source QModelIndex's internal pointer to source QModelIndex
|
||||
QHash<void*, QModelIndex> proxy_to_source_mapping_;
|
||||
unsigned int flags_ = 0;
|
||||
};
|
||||
|
||||
// top-level items
|
||||
@ -93,6 +109,9 @@ public:
|
||||
// if task is removed locally from tasks vector, it is marked with a nullptr
|
||||
std::map<std::string, RemoteTaskModel*> remote_tasks_;
|
||||
|
||||
// factory used to create stages
|
||||
StageFactoryPtr stage_factory_;
|
||||
|
||||
public:
|
||||
TaskListModelPrivate(TaskListModel* q_ptr) : q_ptr(q_ptr) {}
|
||||
|
||||
@ -143,6 +162,8 @@ public:
|
||||
return q_ptr->createIndex(src.row(), src.column(), src_parent.internalPointer());
|
||||
}
|
||||
|
||||
void removeTask(BaseTaskModel *model);
|
||||
|
||||
private:
|
||||
void _q_sourceRowsAboutToBeInserted(const QModelIndex &parent, int start, int end);
|
||||
void _q_sourceRowsInserted(const QModelIndex &parent, int start, int end);
|
||||
@ -159,11 +180,11 @@ TaskListModel::TaskListModel(QObject *parent)
|
||||
: QAbstractItemModel(parent)
|
||||
{
|
||||
d_ptr = new TaskListModelPrivate(this);
|
||||
ROS_DEBUG_NAMED("TaskListModel", "created TaskListModel: %p", this);
|
||||
ROS_DEBUG_NAMED(LOGNAME, "created TaskListModel: %p", this);
|
||||
}
|
||||
|
||||
TaskListModel::~TaskListModel() {
|
||||
ROS_DEBUG_NAMED("TaskListModel", "destroying TaskListModel: %p", this);
|
||||
ROS_DEBUG_NAMED(LOGNAME, "destroying TaskListModel: %p", this);
|
||||
delete d_ptr;
|
||||
}
|
||||
|
||||
@ -217,13 +238,33 @@ Qt::ItemFlags TaskListModel::flags(const QModelIndex &index) const
|
||||
{
|
||||
Q_D(const TaskListModel);
|
||||
|
||||
if (!index.isValid())
|
||||
return QAbstractItemModel::flags(index);
|
||||
if (!index.isValid()) {
|
||||
Qt::ItemFlags f = QAbstractItemModel::flags(index);
|
||||
// dropping at root will create a new task
|
||||
if (d->stage_factory_)
|
||||
f |= Qt::ItemIsDropEnabled;
|
||||
return f;
|
||||
}
|
||||
|
||||
QModelIndex src_index = d->mapToSource(index);
|
||||
return src_index.model()->flags(src_index);
|
||||
}
|
||||
|
||||
void TaskListModel::setStageFactory(const StageFactoryPtr &factory)
|
||||
{
|
||||
Q_D(TaskListModel);
|
||||
d->stage_factory_ = factory;
|
||||
}
|
||||
|
||||
QStringList TaskListModel::mimeTypes() const
|
||||
{
|
||||
Q_D(const TaskListModel);
|
||||
QStringList result;
|
||||
if (d->stage_factory_)
|
||||
result << d->stage_factory_->mimeType();
|
||||
return result;
|
||||
}
|
||||
|
||||
QVariant TaskListModel::headerData(int section, Qt::Orientation orientation, int role) const
|
||||
{
|
||||
if (orientation == Qt::Horizontal && role == Qt::DisplayRole)
|
||||
@ -248,12 +289,6 @@ bool TaskListModel::setData(const QModelIndex &index, const QVariant &value, int
|
||||
return task->model_->setData(src_index, value, role);
|
||||
}
|
||||
|
||||
bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent)
|
||||
{
|
||||
// TODO
|
||||
return false;
|
||||
}
|
||||
|
||||
// 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,
|
||||
@ -266,6 +301,9 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
|
||||
bool created = it_inserted.second;
|
||||
RemoteTaskModel*& remote_task = it_inserted.first->second;
|
||||
|
||||
if (!msg.description.empty() && remote_task && remote_task->taskFlags() & BaseTaskModel::IS_DESTROYED)
|
||||
created = true; // re-create remote task after it was destroyed beforehand
|
||||
|
||||
// empty list indicates, that this remote task is not available anymore
|
||||
if (msg.description.empty()) {
|
||||
if (!remote_task) { // task was already deleted locally
|
||||
@ -273,13 +311,6 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
|
||||
d->remote_tasks_.erase(it_inserted.first);
|
||||
return;
|
||||
}
|
||||
// task is still in use, mark it as destroyed
|
||||
for (auto& t : d->tasks_) {
|
||||
if (t.model_ == remote_task) {
|
||||
t.flags_ |= IS_DESTROYED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else if (created) { // create new task model, if ID was not known before
|
||||
// the model is managed by this instance via Qt's parent-child mechanism
|
||||
remote_task = new RemoteTaskModel(this);
|
||||
@ -292,7 +323,7 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id,
|
||||
|
||||
// insert newly created model into this' model instance
|
||||
if (created) {
|
||||
ROS_DEBUG_NAMED("TaskListModel", "received new Task: %s", msg.id.c_str());
|
||||
ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.id.c_str());
|
||||
insertTask(remote_task, -1);
|
||||
}
|
||||
}
|
||||
@ -328,7 +359,8 @@ BaseTaskModel *TaskListModel::getTask(int row) const
|
||||
return d->tasks_.at(row).model_;
|
||||
}
|
||||
|
||||
void TaskListModel::insertTask(BaseTaskModel* model, int row) {
|
||||
void TaskListModel::insertTask(BaseTaskModel* model, int row)
|
||||
{
|
||||
Q_D(TaskListModel);
|
||||
|
||||
if (row < 0 || (size_t)row > d->tasks_.size())
|
||||
@ -337,16 +369,14 @@ void TaskListModel::insertTask(BaseTaskModel* model, int row) {
|
||||
auto it = d->tasks_.begin();
|
||||
std::advance(it, row);
|
||||
|
||||
ROS_DEBUG_NAMED("TaskListModel", "%p: inserting task: %p", this, model);
|
||||
if (LocalTaskModel* lm = dynamic_cast<LocalTaskModel*>(model))
|
||||
lm->setStageFactory(d->stage_factory_);
|
||||
|
||||
ROS_DEBUG_NAMED(LOGNAME, "%p: inserting task: %p", this, model);
|
||||
beginInsertRows(QModelIndex(), row, row);
|
||||
d->tasks_.insert(it, TaskListModelPrivate::BaseModelData(model));
|
||||
endInsertRows();
|
||||
|
||||
// notice destruction of task
|
||||
if (model->parent() != this)
|
||||
connect(model, &BaseTaskModel::destroyed,
|
||||
[this](QObject* o) { removeTask(static_cast<BaseTaskModel*>(o), false); });
|
||||
|
||||
connect(model, SIGNAL(rowsAboutToBeInserted(QModelIndex,int,int)),
|
||||
this, SLOT(_q_sourceRowsAboutToBeInserted(QModelIndex,int,int)));
|
||||
connect(model, SIGNAL(rowsInserted(QModelIndex,int,int)),
|
||||
@ -363,40 +393,111 @@ void TaskListModel::insertTask(BaseTaskModel* model, int row) {
|
||||
this, SLOT(_q_sourceDataChanged(QModelIndex,QModelIndex,QVector<int>)));
|
||||
}
|
||||
|
||||
bool TaskListModel::removeTask(BaseTaskModel* model, bool disconnect_signals) {
|
||||
bool TaskListModel::removeTask(BaseTaskModel* model)
|
||||
{
|
||||
Q_D(TaskListModel);
|
||||
|
||||
// find row corresponding to model
|
||||
auto it = std::find_if(d->tasks_.begin(), d->tasks_.end(),
|
||||
[model](const TaskListModelPrivate::BaseModelData& data) { return data.model_ == model; });
|
||||
[model](const TaskListModelPrivate::BaseModelData& data) {
|
||||
return data.model_ == model;
|
||||
});
|
||||
if (it == d->tasks_.end())
|
||||
return false; // model not found
|
||||
|
||||
ROS_DEBUG_NAMED("TaskListModel", "%p: removing task: %p", this, model);
|
||||
size_t row = it - d->tasks_.begin();
|
||||
beginRemoveRows(QModelIndex(), row, row);
|
||||
d->tasks_.insert(it, TaskListModelPrivate::BaseModelData(model));
|
||||
endRemoveRows();
|
||||
return removeTasks(it - d->tasks_.begin(), 1);
|
||||
}
|
||||
|
||||
if (disconnect_signals) { // reacting on signal destroyed(), signals are already disconnected
|
||||
disconnect(model, SIGNAL(rowsAboutToBeInserted(QModelIndex,int,int)),
|
||||
this, SLOT(_q_sourceRowsAboutToBeInserted(QModelIndex,int,int)));
|
||||
disconnect(model, SIGNAL(rowsInserted(QModelIndex,int,int)),
|
||||
this, SLOT(_q_sourceRowsInserted(QModelIndex,int,int)));
|
||||
disconnect(model, SIGNAL(rowsAboutToBeRemoved(QModelIndex,int,int)),
|
||||
this, SLOT(_q_sourceRowsAboutToBeRemoved(QModelIndex,int,int)));
|
||||
disconnect(model, SIGNAL(rowsRemoved(QModelIndex,int,int)),
|
||||
this, SLOT(_q_sourceRowsRemoved(QModelIndex,int,int)));
|
||||
disconnect(model, SIGNAL(rowsAboutToBeMoved(QModelIndex,int,int,QModelIndex,int)),
|
||||
this, SLOT(_q_sourceRowsAboutToBeMoved(QModelIndex,int,int,QModelIndex,int)));
|
||||
disconnect(model, SIGNAL(rowsMoved(QModelIndex,int,int,QModelIndex,int)),
|
||||
this, SLOT(_q_sourceRowsMoved(QModelIndex,int,int,QModelIndex,int)));
|
||||
disconnect(model, SIGNAL(dataChanged(QModelIndex,QModelIndex,QVector<int>)),
|
||||
this, SLOT(_q_sourceDataChanged(QModelIndex,QModelIndex,QVector<int>)));
|
||||
}
|
||||
bool TaskListModel::removeTasks(int row, int count)
|
||||
{
|
||||
Q_D(TaskListModel);
|
||||
if (row < 0 || row+count > rowCount())
|
||||
return false;
|
||||
|
||||
auto first = d->tasks_.begin(); std::advance(first, row);
|
||||
auto last = first; std::advance(last, count);
|
||||
beginRemoveRows(QModelIndex(), row, row+count-1);
|
||||
std::for_each(first, last, [d](TaskListModelPrivate::BaseModelData &data) {
|
||||
d->removeTask(data.model_);
|
||||
});
|
||||
d->tasks_.erase(first, last);
|
||||
endRemoveRows();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent)
|
||||
{
|
||||
Q_D(TaskListModel);
|
||||
if (!parent.isValid()) { // top-level items = tasks
|
||||
return removeTasks(row, count);
|
||||
} else {
|
||||
TaskListModelPrivate::BaseModelData *data = nullptr;
|
||||
QModelIndex src_parent = d->mapToSource(parent, &data);
|
||||
return data->model_->removeRows(row, count, src_parent);
|
||||
}
|
||||
}
|
||||
|
||||
bool TaskListModel::dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent)
|
||||
{
|
||||
Q_UNUSED(column);
|
||||
Q_D(TaskListModel);
|
||||
if (!d->stage_factory_)
|
||||
return false;
|
||||
const QString& mime_type = d->stage_factory_->mimeType();
|
||||
if (!mime->hasFormat(mime_type))
|
||||
return false;
|
||||
|
||||
if (!parent.isValid() && mime->hasFormat(mime_type)) {
|
||||
QString error;
|
||||
moveit::task_constructor::Stage* stage
|
||||
= d->stage_factory_->makeRaw(mime->data(mime_type), &error);
|
||||
std::unique_ptr<moveit::task_constructor::ContainerBase> container
|
||||
(dynamic_cast<moveit::task_constructor::ContainerBase*>(stage));
|
||||
if (!container) { // only accept container at root level
|
||||
if (stage) delete stage;
|
||||
return false;
|
||||
}
|
||||
|
||||
// create a new local task using the given container as root
|
||||
insertTask(new LocalTaskModel(std::move(container), this), row);
|
||||
return true;
|
||||
}
|
||||
|
||||
// propagate to corresponding child model
|
||||
TaskListModelPrivate::BaseModelData *data = nullptr;
|
||||
QModelIndex src_parent = d->mapToSource(parent, &data);
|
||||
return data->model_->dropMimeData(mime, action, row, column, src_parent);
|
||||
}
|
||||
|
||||
Qt::DropActions TaskListModel::supportedDragActions() const
|
||||
{
|
||||
return Qt::CopyAction | Qt::MoveAction;
|
||||
}
|
||||
|
||||
void TaskListModelPrivate::removeTask(BaseTaskModel *model)
|
||||
{
|
||||
ROS_DEBUG_NAMED(LOGNAME, "%p: removing task: %p", q_ptr, model);
|
||||
|
||||
QObject::disconnect(model, SIGNAL(rowsAboutToBeInserted(QModelIndex,int,int)),
|
||||
q_ptr, SLOT(_q_sourceRowsAboutToBeInserted(QModelIndex,int,int)));
|
||||
QObject::disconnect(model, SIGNAL(rowsInserted(QModelIndex,int,int)),
|
||||
q_ptr, SLOT(_q_sourceRowsInserted(QModelIndex,int,int)));
|
||||
QObject::disconnect(model, SIGNAL(rowsAboutToBeRemoved(QModelIndex,int,int)),
|
||||
q_ptr, SLOT(_q_sourceRowsAboutToBeRemoved(QModelIndex,int,int)));
|
||||
QObject::disconnect(model, SIGNAL(rowsRemoved(QModelIndex,int,int)),
|
||||
q_ptr, SLOT(_q_sourceRowsRemoved(QModelIndex,int,int)));
|
||||
QObject::disconnect(model, SIGNAL(rowsAboutToBeMoved(QModelIndex,int,int,QModelIndex,int)),
|
||||
q_ptr, SLOT(_q_sourceRowsAboutToBeMoved(QModelIndex,int,int,QModelIndex,int)));
|
||||
QObject::disconnect(model, SIGNAL(rowsMoved(QModelIndex,int,int,QModelIndex,int)),
|
||||
q_ptr, SLOT(_q_sourceRowsMoved(QModelIndex,int,int,QModelIndex,int)));
|
||||
QObject::disconnect(model, SIGNAL(dataChanged(QModelIndex,QModelIndex,QVector<int>)),
|
||||
q_ptr, SLOT(_q_sourceDataChanged(QModelIndex,QModelIndex,QVector<int>)));
|
||||
|
||||
// delete model if we own it
|
||||
if (model->parent() == q_ptr)
|
||||
model->deleteLater();
|
||||
}
|
||||
|
||||
void TaskListModelPrivate::_q_sourceRowsAboutToBeInserted(const QModelIndex &parent, int start, int end)
|
||||
{
|
||||
q_ptr->beginInsertRows(mapFromSource(parent), start, end);
|
||||
@ -443,6 +544,21 @@ void TaskListModelPrivate::_q_sourceDataChanged(const QModelIndex &topLeft, cons
|
||||
q_ptr->dataChanged(mapFromSource(topLeft), mapFromSource(bottomRight), roles);
|
||||
}
|
||||
|
||||
|
||||
TaskListView::TaskListView(QWidget *parent)
|
||||
: QTreeView(parent)
|
||||
{
|
||||
}
|
||||
|
||||
// dropping onto an item, should expand this item
|
||||
void TaskListView::dropEvent(QDropEvent *event)
|
||||
{
|
||||
QModelIndex index = indexAt(event->pos());
|
||||
QTreeView::dropEvent(event);
|
||||
if (event->isAccepted())
|
||||
expand(index);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "moc_task_list_model.cpp"
|
||||
|
||||
@ -36,27 +36,48 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "pluginlib_factory.h"
|
||||
#include <moveit_task_constructor/stage.h>
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_task_constructor/TaskDescription.h>
|
||||
#include <moveit_task_constructor/TaskStatistics.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
|
||||
#include <QAbstractItemModel>
|
||||
#include <QTreeView>
|
||||
#include <memory>
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
/** Base class to represent a single local or remote Task as a Qt model. */
|
||||
class BaseTaskModel : public QAbstractItemModel {
|
||||
Q_OBJECT
|
||||
protected:
|
||||
unsigned int flags_ = 0;
|
||||
|
||||
public:
|
||||
enum TaskModelFlag {
|
||||
IS_DESTROYED = 0x01,
|
||||
IS_INITIALIZED = 0x02,
|
||||
IS_RUNNING = 0x04,
|
||||
};
|
||||
|
||||
BaseTaskModel(QObject *parent = nullptr) : QAbstractItemModel(parent) {}
|
||||
|
||||
int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; }
|
||||
QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
|
||||
|
||||
Qt::ItemFlags flags(const QModelIndex &index) const override;
|
||||
unsigned int taskFlags() const { return flags_; }
|
||||
};
|
||||
|
||||
|
||||
typedef PluginlibFactory<moveit::task_constructor::Stage> StageFactory;
|
||||
typedef std::shared_ptr<StageFactory> StageFactoryPtr;
|
||||
StageFactoryPtr getStageFactory();
|
||||
|
||||
|
||||
class TaskListModelPrivate;
|
||||
/** The TaskListModel maintains a list of multiple BaseTaskModels, local and/or remote.
|
||||
*
|
||||
@ -102,7 +123,14 @@ public:
|
||||
|
||||
/// insert a TaskModel into our list
|
||||
void insertTask(BaseTaskModel* model, int row = -1);
|
||||
bool removeTask(BaseTaskModel* model, bool disconnect_signals = true);
|
||||
bool removeTask(BaseTaskModel* model);
|
||||
bool removeTasks(int row, int count);
|
||||
|
||||
/// providing a StageFactory makes the model accepting drops
|
||||
void setStageFactory(const StageFactoryPtr &factory);
|
||||
QStringList mimeTypes() const override;
|
||||
bool dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent) override;
|
||||
Qt::DropActions supportedDragActions() const override;
|
||||
|
||||
private:
|
||||
Q_PRIVATE_SLOT(d_func(), void _q_sourceRowsAboutToBeInserted(QModelIndex,int,int))
|
||||
@ -117,4 +145,13 @@ private:
|
||||
MOVEIT_CLASS_FORWARD(TaskListModel)
|
||||
typedef std::weak_ptr<TaskListModel> TaskListModelWeakPtr;
|
||||
|
||||
|
||||
class TaskListView : public QTreeView {
|
||||
Q_OBJECT
|
||||
public:
|
||||
TaskListView(QWidget *parent = nullptr);
|
||||
|
||||
void dropEvent(QDropEvent *event) override;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -50,22 +50,16 @@
|
||||
#include <rviz/window_manager_interface.h>
|
||||
#include <rviz/panel_dock_widget.h>
|
||||
#include <ros/console.h>
|
||||
#include <QPointer>
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
typedef PluginlibFactory<moveit::task_constructor::Stage> StageFactory;
|
||||
StageFactory* getStageFactory() {
|
||||
static std::shared_ptr<StageFactory> factory
|
||||
(new StageFactory("moveit_task_constructor",
|
||||
"moveit::task_constructor::Stage"));
|
||||
return factory.get();
|
||||
}
|
||||
|
||||
rviz::PanelDockWidget* getStageDockWidget(rviz::WindowManagerInterface* mgr) {
|
||||
static rviz::PanelDockWidget *widget = nullptr;
|
||||
static QPointer<rviz::PanelDockWidget> widget = nullptr;
|
||||
if (!widget && mgr) { // create widget
|
||||
QTreeView *view = new QTreeView(mgr->getParentWindow());
|
||||
view->setModel(new FactoryModel(getStageFactory(), view));
|
||||
QTreeView *view = new QTreeView();
|
||||
StageFactoryPtr factory = getStageFactory();
|
||||
view->setModel(new FactoryModel(*factory, factory->mimeType(), view));
|
||||
view->expandAll();
|
||||
view->setHeaderHidden(true);
|
||||
view->setDragDropMode(QAbstractItemView::DragOnly);
|
||||
@ -81,9 +75,9 @@ TaskPanel::TaskPanel(QWidget* parent)
|
||||
{
|
||||
Q_D(TaskPanel);
|
||||
// connect signals
|
||||
connect(d->actionNewTask, &QAction::triggered, this, &TaskPanel::onAddTask);
|
||||
connect(d->actionNewStage, &QAction::triggered, this, &TaskPanel::onAddStage);
|
||||
connect(d->actionRemoveStages, &QAction::triggered, this, &TaskPanel::onRemoveStages);
|
||||
connect(d->actionRemoveTaskTreeRows, &QAction::triggered, this, &TaskPanel::removeTaskTreeRows);
|
||||
connect(d->actionAddLocalTask, &QAction::triggered, this, &TaskPanel::addTask);
|
||||
connect(d->button_show_stage_dock_widget, &QToolButton::clicked, this, &TaskPanel::showStageDockWidget);
|
||||
}
|
||||
|
||||
TaskPanel::~TaskPanel()
|
||||
@ -93,16 +87,25 @@ TaskPanel::~TaskPanel()
|
||||
|
||||
TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
|
||||
: q_ptr(q_ptr)
|
||||
, tasks_model(TaskListModelCache::instance().getGlobalModel())
|
||||
, settings(new rviz::PropertyTreeModel(new rviz::Property))
|
||||
, task_list_model(TaskListModelCache::instance().getGlobalModel())
|
||||
, settings(new rviz::PropertyTreeModel(new rviz::Property, q_ptr))
|
||||
{
|
||||
setupUi(q_ptr);
|
||||
initSettings(settings->getRoot());
|
||||
settings_view->setModel(settings);
|
||||
tasks_view->setModel(tasks_model.get());
|
||||
// init tasks view
|
||||
task_list_model->setStageFactory(getStageFactory());
|
||||
tasks_view->setModel(task_list_model.get());
|
||||
|
||||
tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection);
|
||||
tasks_view->setAcceptDrops(true);
|
||||
tasks_view->setDefaultDropAction(Qt::CopyAction);
|
||||
tasks_view->setDropIndicatorShown(true);
|
||||
tasks_view->setDragEnabled(true);
|
||||
|
||||
// init actions
|
||||
tasks_view->addActions({actionNewTask, actionNewStage, actionRemoveStages});
|
||||
tasks_view->addActions({actionRemoveTaskTreeRows, actionAddLocalTask});
|
||||
|
||||
initSettings(settings->getRoot());
|
||||
settings_view->setModel(settings);
|
||||
}
|
||||
|
||||
void TaskPanelPrivate::initSettings(rviz::Property* root)
|
||||
@ -125,24 +128,24 @@ void TaskPanel::load(const rviz::Config& config)
|
||||
d_ptr->settings->getRoot()->load(config);
|
||||
}
|
||||
|
||||
void TaskPanel::onAddTask()
|
||||
void TaskPanel::addTask()
|
||||
{
|
||||
Q_D(TaskPanel);
|
||||
QModelIndex current = d->tasks_view->currentIndex();
|
||||
d_ptr->tasks_model->insertTask(new LocalTaskModel(this), current.row());
|
||||
d_ptr->task_list_model->insertTask(new LocalTaskModel(d_ptr->task_list_model.get()), current.row());
|
||||
}
|
||||
|
||||
void TaskPanel::onAddStage()
|
||||
void TaskPanel::showStageDockWidget()
|
||||
{
|
||||
rviz::PanelDockWidget *dock = getStageDockWidget(vis_manager_->getWindowManager());
|
||||
dock->show();
|
||||
}
|
||||
|
||||
void TaskPanel::onRemoveStages()
|
||||
void TaskPanel::removeTaskTreeRows()
|
||||
{
|
||||
Q_D(TaskPanel);
|
||||
for (const auto &range : d_ptr->tasks_view->selectionModel()->selection())
|
||||
d_ptr->tasks_model->removeRows(range.top(), range.bottom()-range.top()+1, range.parent());
|
||||
d_ptr->task_list_model->removeRows(range.top(), range.bottom()-range.top()+1, range.parent());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -69,9 +69,9 @@ public:
|
||||
void save(rviz::Config config) const override;
|
||||
|
||||
public Q_SLOTS:
|
||||
void onAddTask();
|
||||
void onAddStage();
|
||||
void onRemoveStages();
|
||||
void addTask();
|
||||
void showStageDockWidget();
|
||||
void removeTaskTreeRows();
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -30,7 +30,7 @@
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QToolButton" name="button_add_stage">
|
||||
<widget class="QToolButton" name="button_show_stage_dock_widget">
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>18</width>
|
||||
@ -49,7 +49,7 @@
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QTreeView" name="tasks_view">
|
||||
<widget class="moveit_rviz_plugin::TaskListView" name="tasks_view">
|
||||
<property name="contextMenuPolicy">
|
||||
<enum>Qt::ActionsContextMenu</enum>
|
||||
</property>
|
||||
@ -66,36 +66,23 @@
|
||||
<widget class="rviz::PropertyTreeWidget" name="settings_view"/>
|
||||
</item>
|
||||
</layout>
|
||||
<action name="actionNewStage">
|
||||
<property name="icon">
|
||||
<iconset theme="add">
|
||||
<normaloff>.</normaloff>.</iconset>
|
||||
</property>
|
||||
<action name="actionRemoveTaskTreeRows">
|
||||
<property name="text">
|
||||
<string>New Stage</string>
|
||||
<string>Remove</string>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Add a new stage</string>
|
||||
<string>Remove selected rows</string>
|
||||
</property>
|
||||
<property name="shortcut">
|
||||
<string>Del</string>
|
||||
</property>
|
||||
<property name="shortcutContext">
|
||||
<enum>Qt::WidgetShortcut</enum>
|
||||
</property>
|
||||
</action>
|
||||
<action name="actionNewTask">
|
||||
<property name="icon">
|
||||
<iconset theme="add">
|
||||
<normaloff>.</normaloff>.</iconset>
|
||||
</property>
|
||||
<action name="actionAddLocalTask">
|
||||
<property name="text">
|
||||
<string>New Task</string>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Add a new task</string>
|
||||
</property>
|
||||
</action>
|
||||
<action name="actionRemoveStages">
|
||||
<property name="text">
|
||||
<string>Remove Stages</string>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Remove selected stages</string>
|
||||
<string>Add task</string>
|
||||
</property>
|
||||
</action>
|
||||
</widget>
|
||||
@ -105,24 +92,12 @@
|
||||
<extends>QTreeView</extends>
|
||||
<header location="global">rviz/properties/property_tree_widget.h</header>
|
||||
</customwidget>
|
||||
<customwidget>
|
||||
<class>moveit_rviz_plugin::TaskListView</class>
|
||||
<extends>QTreeView</extends>
|
||||
<header>task_list_model.h</header>
|
||||
</customwidget>
|
||||
</customwidgets>
|
||||
<resources/>
|
||||
<connections>
|
||||
<connection>
|
||||
<sender>button_add_stage</sender>
|
||||
<signal>clicked()</signal>
|
||||
<receiver>actionNewStage</receiver>
|
||||
<slot>trigger()</slot>
|
||||
<hints>
|
||||
<hint type="sourcelabel">
|
||||
<x>378</x>
|
||||
<y>21</y>
|
||||
</hint>
|
||||
<hint type="destinationlabel">
|
||||
<x>-1</x>
|
||||
<y>-1</y>
|
||||
</hint>
|
||||
</hints>
|
||||
</connection>
|
||||
</connections>
|
||||
<connections/>
|
||||
</ui>
|
||||
|
||||
@ -53,7 +53,7 @@ public:
|
||||
void initSettings(rviz::Property *root);
|
||||
|
||||
TaskPanel* q_ptr;
|
||||
TaskListModelPtr tasks_model;
|
||||
TaskListModelPtr task_list_model;
|
||||
rviz::PropertyTreeModel* settings;
|
||||
};
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user