TaskPanel: listen to task_monitoring topic

This commit is contained in:
Robert Haschke 2017-10-24 17:01:24 +02:00
parent abe61ef9eb
commit 8a461af97c
25 changed files with 915 additions and 28 deletions

View File

@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
moveit_ros_planning_interface
moveit_msgs
eigen_conversions
rviz
)
# ROS messages, services and actions
@ -58,6 +59,16 @@ add_library(${PROJECT_NAME}
)
add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
install(DIRECTORY include/ DESTINATION include)
install(FILES
task_panel_rviz_plugin_description.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
add_subdirectory(src/stages)
add_subdirectory(src/demo)
add_subdirectory(src/test)
add_subdirectory(visualization)

View File

@ -13,6 +13,9 @@ namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(Task)
MOVEIT_CLASS_FORWARD(SolutionBase)
#define DEFAULT_TASK_MONITOR_TOPIC "task_monitoring"
#define DEFAULT_TASK_SOLUTION_TOPIC "task_solutions"
class Introspection {
// publish Task state and (new) Solutions
ros::Publisher task_publisher_;
@ -22,8 +25,8 @@ class Introspection {
ros::ServiceServer get_solution_service_;
public:
Introspection(const std::string &task_topic = "task",
const std::string &solution_topic = "task_plan");
Introspection(const std::string &task_topic = DEFAULT_TASK_MONITOR_TOPIC,
const std::string &solution_topic = DEFAULT_TASK_SOLUTION_TOPIC);
Introspection(const Introspection &other) = delete;
static Introspection &instance();

View File

@ -28,6 +28,19 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory)
namespace moveit { namespace task_constructor {
enum InterfaceFlag {
READS_START = 0x01,
READS_END = 0x02,
WRITES_NEXT_START = 0x04,
WRITES_PREV_END = 0x08,
OWN_IF_MASK = READS_START | READS_END,
EXT_IF_MASK = WRITES_NEXT_START | WRITES_PREV_END,
INPUT_IF_MASK = READS_START | WRITES_PREV_END,
OUTPUT_IF_MASK = READS_END | WRITES_NEXT_START,
};
typedef Flags<InterfaceFlag> InterfaceFlags;
MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(Stage)
class InterfaceState;

View File

@ -1,5 +1,5 @@
# unique of this task
string id
# list of all stages, include the task stage itself
# list of all stages, including the task stage itself
Stage[] stages

View File

@ -1,13 +1,27 @@
<package>
<name>moveit_task_constructor</name>
<version>0.0.0</version>
<description>package</description>
<description>MoveIt Task Pipeline</description>
<license>BSD</license>
<maintainer email="me@v4hn.de">Michael Goerner</maintainer>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>moveit_core</build_depend>
<build_depend>moveit_ros_planning</build_depend>
<build_depend>moveit_ros_planning_interface</build_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>rviz</build_depend>
<run_depend>moveit_core</run_depend>
<run_depend>moveit_msgs</run_depend>
<run_depend>rviz</run_depend>
<test_depend>rosunit</test_depend>
<export>
<rviz plugin="${prefix}/task_panel_rviz_plugin_description.xml"/>
</export>
</package>

View File

@ -1,5 +1,5 @@
add_executable(plan_pick_ur5 plan_pick_ur5.cpp)
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages ${PROJECT_NAME})
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages)
add_executable(plan_pick_trixi plan_pick_trixi.cpp)
target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_stages ${PROJECT_NAME})
target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_stages)

View File

@ -8,7 +8,7 @@
namespace moveit { namespace task_constructor {
Introspection::Introspection(const std::string &task_topic,
const std::string &solution_topic)
const std::string &solution_topic)
{
ros::NodeHandle n;
task_publisher_ = n.advertise<moveit_task_constructor::Task>(task_topic, 1);

View File

@ -12,20 +12,6 @@
namespace moveit { namespace task_constructor {
enum InterfaceFlag {
READS_START = 0x01,
READS_END = 0x02,
WRITES_NEXT_START = 0x04,
WRITES_PREV_END = 0x08,
OWN_IF_MASK = READS_START | READS_END,
EXT_IF_MASK = WRITES_NEXT_START | WRITES_PREV_END,
INPUT_IF_MASK = READS_START | WRITES_PREV_END,
OUTPUT_IF_MASK = READS_END | WRITES_NEXT_START,
};
typedef Flags<InterfaceFlag> InterfaceFlags;
class ContainerBase;
class StagePrivate {
friend class Stage;

View File

@ -11,4 +11,4 @@ add_library(${PROJECT_NAME}_stages
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/generate_grasp_pose.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/cartesian_position_motion.h
)
target_link_libraries(${PROJECT_NAME}_stages ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})

View File

@ -204,7 +204,7 @@ inline const ContainerBase* Task::wrapped() const
std::string Task::id() const
{
ros::NodeHandle n;
ros::NodeHandle n("~");
return std::to_string(id_) + '@' + n.getNamespace();
}

View File

@ -7,8 +7,11 @@ include_directories(${CMAKE_SOURCE_DIR}/src)
## Add gtest based cpp test target and link libraries
if (CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-container_test container.cpp)
target_link_libraries(${PROJECT_NAME}-container_test ${PROJECT_NAME} gtest_main ${catkin_LIBRARIES})
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_main ${catkin_LIBRARIES})
catkin_add_gtest(${PROJECT_NAME}-test-task_model test_task_model.cpp)
target_link_libraries(${PROJECT_NAME}-test-task_model moveit_task_panel_rviz_plugin gtest_main)
endif()
add_executable(test_plan_current_state test_plan_current_state.cpp)

View File

@ -4,7 +4,7 @@
#include <gtest/gtest.h>
#include <initializer_list>
namespace moveit { namespace task_constructor {
using namespace moveit::task_constructor;
class TestGenerator : public Generator {
public:
@ -87,5 +87,3 @@ TEST_F(BaseTest, serialContainer) {
ASSERT_TRUE(c.insert(std::make_unique<TestGenerator>()));
EXPECT_THROW(c.init(scene), InitStageException);
}
} }

View File

@ -0,0 +1,137 @@
#include <task_model.h>
#include <gtest/gtest.h>
#include <initializer_list>
class TaskModelTest : public ::testing::Test {
protected:
moveit_rviz_plugin::TaskModel model;
int children = 0;
int num_inserts = 0;
int num_updates = 0;
moveit_task_constructor::Task genMsg(const std::string &name) {
moveit_task_constructor::Task t;
t.id = name;
uint id = 0;
moveit_task_constructor::Stage root;
root.parent_id = id;
root.id = ++id;
root.name = name;
t.stages.push_back(root);
for (int i = 0; i != children; ++i) {
moveit_task_constructor::Stage child;
child.parent_id = root.id;
child.id = ++id;
child.name = std::to_string(i);
t.stages.push_back(child);
}
return t;
}
void validate(const std::initializer_list<const char*> &expected) {
// validate root index
ASSERT_EQ(model.rowCount(), expected.size());
EXPECT_EQ(model.columnCount(), 3);
EXPECT_EQ(model.parent(QModelIndex()), QModelIndex());
// validate first-level items
auto it = expected.begin();
for (size_t row = 0; row < expected.size(); ++row, ++it) {
QModelIndex idx = model.index(row, 0);
EXPECT_EQ(idx.row(), row);
EXPECT_EQ(idx.column(), 0);
EXPECT_EQ(idx.model(), &model);
EXPECT_EQ(model.parent(idx), QModelIndex());
// validate data
EXPECT_STREQ(model.data(idx).toByteArray().constData(), *it);
EXPECT_EQ(model.data(model.index(row, 1)).toUInt(), 0);
EXPECT_EQ(model.data(model.index(row, 2)).toUInt(), 0);
// validate children
ASSERT_EQ(model.rowCount(idx), children);
EXPECT_EQ(model.columnCount(idx), 3);
EXPECT_EQ(model.rowCount(model.index(row, 1)), 0);
// validate second-level items
for (int child = 0; child < children; ++child) {
QModelIndex childIdx = model.index(child, 0, idx);
EXPECT_EQ(childIdx.row(), child);
EXPECT_EQ(childIdx.column(), 0);
EXPECT_EQ(childIdx.model(), &model);
EXPECT_EQ(model.parent(childIdx), idx);
EXPECT_EQ(model.data(childIdx).toString().toStdString(), std::to_string(child));
EXPECT_EQ(model.rowCount(childIdx), 0);
EXPECT_EQ(model.columnCount(childIdx), 3);
}
}
}
void populateAndValidate() {
{ SCOPED_TRACE("empty"); validate({}); }
EXPECT_EQ(num_inserts, 0);
EXPECT_EQ(num_updates, 0);
for (int i = 0; i < 2; ++i) {
SCOPED_TRACE("first i=" + std::to_string(i));
num_inserts = 0;
num_updates = 0;
model.processTaskMessage(genMsg("first"));
if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task
else EXPECT_EQ(num_inserts, 0);
EXPECT_EQ(num_updates, 0);
validate({"first"});
}
for (int i = 0; i < 2; ++i) {
SCOPED_TRACE("second i=" + std::to_string(i));
num_inserts = 0;
num_updates = 0;
model.processTaskMessage(genMsg("second")); // 1 notify for inserted task
if (i == 0) EXPECT_EQ(num_inserts, 1);
else EXPECT_EQ(num_inserts, 0);
EXPECT_EQ(num_updates, 0);
validate({"first", "second"});
}
}
void SetUp() {
QObject::connect(&model, &QAbstractItemModel::rowsAboutToBeInserted,
[this](){ ++num_inserts; });
QObject::connect(&model, &QAbstractItemModel::dataChanged,
[this](){ ++num_updates; });
}
void TearDown() {}
};
TEST_F(TaskModelTest, noChildren) {
children = 0;
populateAndValidate();
}
TEST_F(TaskModelTest, threeChildren) {
children = 3;
populateAndValidate();
}
TEST_F(TaskModelTest, visitedPopulate) {
// first population without children
children = 0;
model.processTaskMessage(genMsg("first"));
validate({"first"}); // validation visits root node
EXPECT_EQ(num_inserts, 1);
children = 3;
num_inserts = 0;
model.processTaskMessage(genMsg("first"));
validate({"first"});
// second population with children should emit insert notifies for them
EXPECT_EQ(num_inserts, 3);
EXPECT_EQ(num_updates, 0);
}

View File

@ -0,0 +1,9 @@
<library path="lib/libmoveit_task_panel_rviz_plugin">
<class name="moveit_task_constructor/Motion Planning Tasks"
type="moveit_rviz_plugin::TaskPanel"
base_class_type="rviz::Panel">
<description>
A panel widget to visualize and edit planning tasks
</description>
</class>
</library>

View File

@ -0,0 +1,23 @@
# definition needed for boost/math/constants/constants.hpp included by Ogre to compile
add_definitions(-DBOOST_MATH_DISABLE_FLOAT128)
# Qt Stuff
if(rviz_QT_VERSION VERSION_LESS "5")
find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui)
include(${QT_USE_FILE})
macro(qt_wrap_ui)
qt4_wrap_ui(${ARGN})
endmacro()
else()
find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
set(QT_LIBRARIES Qt5::Widgets)
macro(qt_wrap_ui)
qt5_wrap_ui(${ARGN})
endmacro()
endif()
set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_AUTOMOC ON)
add_definitions(-DQT_NO_KEYWORDS)
add_subdirectory(task_panel)

View File

@ -0,0 +1,20 @@
set(MOVEIT_LIB_NAME moveit_task_panel_rviz_plugin)
qt_wrap_ui(UIC_FILES task_panel.ui)
add_library(${MOVEIT_LIB_NAME}
mainloop_processing.cpp
stage_wrapper.cpp
task_model.cpp
task_panel.cpp
${UIC_FILES}
)
target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${QT_LIBRARIES})
target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
PRIVATE ${CMAKE_CURRENT_BINARY_DIR}
)
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

View File

@ -0,0 +1,55 @@
#include "mainloop_processing.h"
#include <ros/console.h>
namespace moveit { namespace tools {
MainLoopProcessing::MainLoopProcessing(QObject *parent) : QObject(parent)
{
}
void MainLoopProcessing::addJob(const std::function<void()>& job)
{
boost::unique_lock<boost::mutex> ulock(jobs_mutex_);
jobs_.push_back(job);
}
void MainLoopProcessing::clear()
{
jobs_.clear();
}
size_t MainLoopProcessing::numPending()
{
boost::unique_lock<boost::mutex> ulock(jobs_mutex_);
return jobs_.size();
}
void MainLoopProcessing::waitForAllJobs()
{
boost::unique_lock<boost::mutex> ulock(jobs_mutex_);
while (!jobs_.empty())
idle_condition_.wait(ulock);
}
void MainLoopProcessing::executeJobs()
{
boost::unique_lock<boost::mutex> ulock(jobs_mutex_);
while (!jobs_.empty())
{
std::function<void()> fn = jobs_.front();
jobs_.pop_front();
ulock.unlock();
try
{
fn();
}
catch (std::exception& ex)
{
ROS_ERROR("Exception caught executing main loop job: %s", ex.what());
}
ulock.lock();
}
idle_condition_.notify_all();
}
} }

View File

@ -0,0 +1,29 @@
#pragma once
#include <QObject>
#include <QObject>
#include <deque>
#include <functional>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>
namespace moveit { namespace tools {
class MainLoopProcessing : public QObject
{
Q_OBJECT
boost::mutex jobs_mutex_;
std::deque<std::function<void()> > jobs_;
boost::condition_variable idle_condition_;
public:
explicit MainLoopProcessing(QObject *parent = 0);
void addJob(const std::function<void()> &job);
void clear();
size_t numPending();
void waitForAllJobs();
void executeJobs();
};
} }

View File

@ -0,0 +1,35 @@
#include <stdio.h>
#include "stage_wrapper.h"
#include <moveit_task_constructor/Task.h>
namespace moveit_rviz_plugin {
size_t RemoteStage::numChildren() const
{
return children_.size();
}
RemoteStage *RemoteStage::child(size_t index)
{
return children_.at(index).get();
}
bool RemoteStage::setName(const std::string &name)
{
if (name == name_) return false;
name_ = name;
return true; // indicate change
}
size_t RemoteStage::numSolved() const
{
return 0;
}
size_t RemoteStage::numFailed() const
{
return 0;
}
}

View File

@ -0,0 +1,70 @@
#pragma once
#include <vector>
#include <map>
#include <memory>
#include <qflags.h>
namespace moveit_rviz_plugin {
enum StageFlag {
READS_START = 0x01,
READS_END = 0x02,
WRITES_NEXT_START = 0x04,
WRITES_PREV_END = 0x08,
IS_EDITABLE = 0x10,
IS_DESTROYED = 0x20, // indicate that creator of this stage was destroyed
WAS_VISITED = 0x40, // indicate that model should emit change notifications
};
typedef QFlags<StageFlag> StageFlags;
/** Wrapper class used in QAbstractItemModel of rviz::Panel to show the task tree */
class StageWrapperInterface {
public:
virtual ~StageWrapperInterface() {}
virtual StageWrapperInterface* parent() = 0;
virtual size_t numChildren() const = 0;
virtual StageWrapperInterface* child(size_t index) = 0;
virtual const std::string& name() const = 0;
virtual bool setName(const std::string& name) = 0;
virtual size_t numSolved() const = 0;
virtual size_t numFailed() const = 0;
mutable StageFlags flags;
};
class RemoteStage : public StageWrapperInterface {
std::string name_;
RemoteStage *parent_;
std::vector<std::unique_ptr<RemoteStage>> children_;
public:
RemoteStage(RemoteStage *parent) : parent_(parent) {}
RemoteStage* parent() override { return parent_; }
size_t numChildren() const override;
RemoteStage* child(size_t index) override;
void push_back(std::unique_ptr<RemoteStage>&& child) {
children_.push_back(std::move(child));
}
const std::string& name() const override { return name_; }
bool setName(const std::string& name) override;
size_t numSolved() const override;
size_t numFailed() const override;
};
// root stage of a tree of remote stages
class RemoteTask : public RemoteStage {
friend class TaskModel;
std::map<uint32_t, RemoteStage*> id_to_stage;
public:
RemoteTask() : RemoteStage(nullptr) {}
};
}

View File

@ -0,0 +1,248 @@
#include "task_model.h"
#include "stage_wrapper.h"
#include <moveit_task_constructor/stage.h>
#include <ros/console.h>
namespace moveit_rviz_plugin {
class TaskModelPrivate {
public:
typedef std::vector<std::unique_ptr<StageWrapperInterface>> task_array_type;
TaskModel* q_ptr;
// the vector of all visible tasks in the model
task_array_type tasks;
// map from remote task IDs to tasks
// if task is destroyed remotely, it is marked with flag IS_DESTROYED
// if task is removed locally from tasks vector, it is marked with a nullptr
std::map<std::string, RemoteTask*> remote_tasks;
public:
TaskModelPrivate(TaskModel* q_ptr) : q_ptr(q_ptr) {}
inline bool indexValid(const QModelIndex &index) const {
return (index.row() >= 0) && (index.column() >= 0) && (index.model() == q_ptr);
}
StageWrapperInterface *node(const QModelIndex &index) const;
QModelIndex index(StageWrapperInterface *node) const;
};
TaskModel::TaskModel(QObject *parent)
: QAbstractItemModel(parent)
, d_ptr (new TaskModelPrivate(this))
{}
TaskModel::~TaskModel() { delete d_ptr; }
int TaskModel::rowCount(const QModelIndex &parent) const
{
Q_D(const TaskModel);
if (parent.column() > 0)
return 0;
if (!parent.isValid())
return d_ptr->tasks.size();
const StageWrapperInterface* parentNode = d->node(parent);
parentNode->flags |= WAS_VISITED;
return parentNode->numChildren();
}
QModelIndex TaskModel::index(int row, int column, const QModelIndex &parent) const
{
Q_D(const TaskModel);
if (row < 0 || column < 0 || row >= rowCount(parent) || column >= columnCount(parent))
return QModelIndex();
StageWrapperInterface* indexNode
= (d->indexValid(parent)
? d->node(parent)->child(row)
: d->tasks[row].get());
Q_ASSERT(indexNode);
return createIndex(row, column, indexNode);
}
inline StageWrapperInterface* TaskModelPrivate::node(const QModelIndex &index) const
{
Q_ASSERT(index.isValid());
return static_cast<StageWrapperInterface*>(index.internalPointer());
}
QModelIndex TaskModel::parent(const QModelIndex &index) const
{
Q_D(const TaskModel);
if (!d->indexValid(index))
return QModelIndex();
StageWrapperInterface* indexNode = d->node(index);
return d->index(indexNode->parent());
}
QModelIndex TaskModelPrivate::index(StageWrapperInterface* node) const
{
if (node == nullptr) return QModelIndex();
// get parent's row
int row;
StageWrapperInterface* parentNode = node->parent();
if (parentNode == nullptr) {
auto it = std::find_if(tasks.begin(), tasks.end(),
[node](task_array_type::const_reference n) { return n.get() == node; });
Q_ASSERT(it != tasks.end());
row = it - tasks.begin();
} else {
int end = parentNode->numChildren();
for (row = 0; row != end; ++row)
if (parentNode->child(row) == node)
break;
Q_ASSERT(row != end);
}
return q_ptr->createIndex(row, 0, node);
}
Qt::ItemFlags TaskModel::flags(const QModelIndex &index) const
{
Q_D(const TaskModel);
Qt::ItemFlags flags = QAbstractItemModel::flags(index);
if (index.column() == 0)
flags |= Qt::ItemIsEditable; // name is editable
return flags;
}
QVariant TaskModel::headerData(int section, Qt::Orientation orientation, int role) const
{
if (orientation == Qt::Horizontal && role == Qt::DisplayRole) {
switch (section) {
case 0: return tr("Name");
case 1: return tr("# solved");
case 2: return tr("# failed");
}
}
return QAbstractItemModel::headerData(section, orientation, role);
}
QVariant TaskModel::data(const QModelIndex &index, int role) const
{
Q_D(const TaskModel);
if (!d->indexValid(index))
return QVariant();
StageWrapperInterface* indexNode = d->node(index);
switch (role) {
case Qt::EditRole:
case Qt::DisplayRole:
switch (index.column()) {
case 0: return QString::fromStdString(indexNode->name());
case 1: return uint(indexNode->numSolved());
case 2: return uint(indexNode->numFailed());
}
break;
}
return QVariant();
}
bool TaskModel::setData(const QModelIndex &index, const QVariant &value, int role)
{
Q_D(const TaskModel);
if (!d->indexValid(index) || index.column() != 0 || role != Qt::EditRole)
return false;
StageWrapperInterface* indexNode = d->node(index);
return indexNode->setName(value.toString().toStdString());
}
// process a task monitoring message:
// update existing RemoteTask, create a new one,
// or (if msg.stages is empty) delete an existing one
void TaskModel::processTaskMessage(const moveit_task_constructor::Task &msg)
{
Q_D(TaskModel);
// retrieve existing or insert new remote task for given id
auto it_inserted = d->remote_tasks.insert(std::make_pair(msg.id, nullptr));
bool created = it_inserted.second;
if (created) // create new task, if ID was not known before
it_inserted.first->second = new RemoteTask();
RemoteTask *remote_task = it_inserted.first->second;
// empty stages list indicates, that this remote task is not available anymore
if (msg.stages.empty()) {
// task was already deleted locally, now we can remove it from remote_tasks
if (!remote_task) d->remote_tasks.erase(it_inserted.first);
// task is still in use, mark it as destroyed
else remote_task->flags |= IS_DESTROYED;
}
if (!remote_task) return; // task is not in use anymore
processTaskMessage(remote_task, msg);
// notify model about newly created task
if (created) {
int row = rowCount();
beginInsertRows(QModelIndex(), row, row);
d->tasks.push_back(std::unique_ptr<RemoteTask>(remote_task));
endInsertRows();
}
}
typedef moveit_task_constructor::Stage StageMsg;
static std::map<StageFlag, moveit::task_constructor::InterfaceFlags>
remote_stage_flags = {
{ READS_START, moveit::task_constructor::READS_START },
{ READS_END,moveit::task_constructor::READS_END },
{ WRITES_NEXT_START, moveit::task_constructor::WRITES_NEXT_START },
{ WRITES_PREV_END, moveit::task_constructor::WRITES_PREV_END },
};
void TaskModel::processTaskMessage(RemoteTask* root, const moveit_task_constructor::Task &msg)
{
Q_D(TaskModel);
// iterate over msg.stages and create new nodes where needed
for (const StageMsg &s : msg.stages) {
RemoteStage *parent = nullptr;
// find parent for any non-root stage
if (s.parent_id != 0) {
auto parent_it = root->id_to_stage.find(s.parent_id);
if (parent_it == root->id_to_stage.end()) {
ROS_ERROR_NAMED("TaskModel", "No parent found for stage %d (%s)", s.id, s.name.c_str());
continue;
}
parent = parent_it->second;
}
auto it_inserted = root->id_to_stage.insert(std::make_pair(s.id, s.parent_id == 0 ? root : nullptr));
RemoteStage*& stage = it_inserted.first->second;
if (!stage) // if just inserted, create new stage
stage = new RemoteStage(parent);
else
assert (stage->parent() == parent || stage == root);
// set content of stage
bool changed = stage->setName(s.name);
StageFlags old = stage->flags;
for (const auto &pair : remote_stage_flags) {
if (s.id & pair.second)
stage->flags |= pair.first;
else
stage->flags &= ~pair.first;
}
changed |= (stage->flags != old);
// notify model about changes to stage (if neccessary)
if (changed && (stage->flags & WAS_VISITED)) {
QModelIndex idx = d->index(stage);
dataChanged(idx, idx);
}
// insert newly created stage into model (root insertion is handled externally)
if (it_inserted.second && stage != root) {
bool notify = parent->flags & WAS_VISITED;
QModelIndex parentIdx = notify ? d->index(parent) : QModelIndex();
int row = rowCount(parentIdx);
if (notify) beginInsertRows(parentIdx, row, row);
parent->push_back(std::unique_ptr<RemoteStage>(stage));
if (notify) endInsertRows();
}
}
}
}

View File

@ -0,0 +1,40 @@
#pragma once
#include <QAbstractItemModel>
#include <moveit_task_constructor/Task.h>
#include <memory>
namespace moveit_rviz_plugin {
class RemoteTask;
class TaskModelPrivate;
class TaskModel : public QAbstractItemModel
{
Q_OBJECT
Q_DECLARE_PRIVATE(TaskModel)
TaskModelPrivate* d_ptr;
public:
TaskModel(QObject *parent = nullptr);
~TaskModel();
int rowCount(const QModelIndex &parent = QModelIndex()) const override;
int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; }
QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override;
QModelIndex parent(const QModelIndex &index) const override;
Qt::ItemFlags flags(const QModelIndex & index) const override;
QVariant headerData(int section, Qt::Orientation orientation, int role) 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;
/// process an incoming task message
void processTaskMessage(const moveit_task_constructor::Task &msg);
private:
void processTaskMessage(RemoteTask *root, const moveit_task_constructor::Task &msg);
};
}

View File

@ -0,0 +1,115 @@
#include <stdio.h>
#include "task_panel.h"
#include "ui_task_panel.h"
#include "task_model.h"
#include <moveit_task_constructor/Task.h>
#include <moveit_task_constructor/introspection.h>
#include <moveit/background_processing/background_processing.h>
#include "mainloop_processing.h"
#include <ros/ros.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/property_tree_model.h>
#include <rviz/properties/property_tree_widget.h>
#include <QTimer>
namespace moveit_rviz_plugin {
class TaskPanelPrivate : public Ui_TaskPanel {
public:
TaskPanelPrivate(TaskPanel *q_ptr);
void initSettings(rviz::Property *root);
void processTaskMessage(const moveit_task_constructor::TaskConstPtr &msg);
// private slots
void _q_changedTaskMonitorTopic();
TaskPanel* q_ptr;
ros::NodeHandle nh;
ros::Subscriber task_monitor_sub;
// task model
TaskModel* tasks_model;
// settings
rviz::PropertyTreeModel* settings;
rviz::RosTopicProperty* task_monitor_topic_property_;
moveit::tools::BackgroundProcessing background_process_;
moveit::tools::MainLoopProcessing mainloop_jobs_;
QTimer mainloop_timer_; // timer to trigger mainloop jobs;
};
TaskPanel::TaskPanel(QWidget* parent)
: rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this))
{
}
TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
: q_ptr(q_ptr)
, tasks_model(new TaskModel(q_ptr))
, settings(new rviz::PropertyTreeModel(new rviz::Property))
{
setupUi(q_ptr);
initSettings(settings->getRoot());
settings_view->setModel(settings);
tasks_view->setModel(tasks_model);
// frequently run mainloop jobs
QObject::connect(&mainloop_timer_, &QTimer::timeout,
&mainloop_jobs_, &moveit::tools::MainLoopProcessing::executeJobs);
mainloop_timer_.start(100);
}
void TaskPanelPrivate::initSettings(rviz::Property* root)
{
task_monitor_topic_property_ =
new rviz::RosTopicProperty("Task Monitor Topic", DEFAULT_TASK_MONITOR_TOPIC,
ros::message_traits::datatype<moveit_task_constructor::Task>(),
"The topic on which task updates (moveit_msgs::Task messages) are received",
root, SLOT(_q_changedTaskMonitorTopic()), q_ptr);
}
void TaskPanelPrivate::_q_changedTaskMonitorTopic()
{
task_monitor_sub.shutdown();
if (!task_monitor_topic_property_->getStdString().empty()) {
task_monitor_sub = nh.subscribe(task_monitor_topic_property_->getStdString(), 2,
&TaskPanelPrivate::processTaskMessage, this);
}
}
void TaskPanelPrivate::processTaskMessage(const moveit_task_constructor::TaskConstPtr& msg) {
// tasks_model needs to be modified in main loop
mainloop_jobs_.addJob([this, msg](){ tasks_model->processTaskMessage(*msg); });
}
void TaskPanel::onInitialize()
{
d_ptr->_q_changedTaskMonitorTopic();
}
void TaskPanel::save(rviz::Config config) const
{
rviz::Panel::save(config);
d_ptr->settings->getRoot()->save(config);
}
void TaskPanel::load(const rviz::Config& config)
{
rviz::Panel::load(config);
d_ptr->settings->getRoot()->load(config);
}
}
#include "moc_task_panel.cpp"
// Tell pluginlib about this class. Every class which should be
// loadable by pluginlib::ClassLoader must have these two lines
// compiled in its .cpp file, outside of any namespace scope.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz::Panel)

View File

@ -0,0 +1,30 @@
#pragma once
#include <rviz/panel.h>
namespace moveit_rviz_plugin {
/** The TaskPanel displays information about manipulation tasks in the system.
* Subscribing to task_monitoring and task_solution topics, it collects information
* about running tasks and their solutions and allows to inspect both,
* successful solutions and failed solution attempts.
*/
class TaskPanelPrivate;
class TaskPanel: public rviz::Panel
{
Q_OBJECT
Q_DECLARE_PRIVATE(TaskPanel)
TaskPanelPrivate *d_ptr;
public:
TaskPanel(QWidget* parent = 0);
void onInitialize() override;
void load(const rviz::Config& config) override;
void save(rviz::Config config) const override;
private:
Q_PRIVATE_SLOT(d_func(), void _q_changedTaskMonitorTopic())
};
}

View File

@ -0,0 +1,48 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>moveit_rviz_plugin::TaskPanel</class>
<widget class="QWidget" name="moveit_rviz_plugin::TaskPanel">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<height>300</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QLabel" name="tasks_view_label">
<property name="text">
<string>Task Tree</string>
</property>
</widget>
</item>
<item>
<widget class="QTreeView" name="tasks_view"/>
</item>
<item>
<widget class="QLabel" name="settings_view_label">
<property name="text">
<string>Settings</string>
</property>
</widget>
</item>
<item>
<widget class="rviz::PropertyTreeWidget" name="settings_view"/>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>rviz::PropertyTreeWidget</class>
<extends>QTreeView</extends>
<header location="global">rviz/properties/property_tree_widget.h</header>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>