diff --git a/CMakeLists.txt b/CMakeLists.txt index 7904e0eb..02da878e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/include/moveit_task_constructor/introspection.h b/include/moveit_task_constructor/introspection.h index c3e50f57..3839c84b 100644 --- a/include/moveit_task_constructor/introspection.h +++ b/include/moveit_task_constructor/introspection.h @@ -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(); diff --git a/include/moveit_task_constructor/stage.h b/include/moveit_task_constructor/stage.h index cb2496f6..2f9657ec 100644 --- a/include/moveit_task_constructor/stage.h +++ b/include/moveit_task_constructor/stage.h @@ -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 InterfaceFlags; + MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Stage) class InterfaceState; diff --git a/msg/Task.msg b/msg/Task.msg index 7c8f5d70..1c641868 100644 --- a/msg/Task.msg +++ b/msg/Task.msg @@ -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 diff --git a/package.xml b/package.xml index 7cde1b48..b1d727a8 100644 --- a/package.xml +++ b/package.xml @@ -1,13 +1,27 @@ moveit_task_constructor 0.0.0 - package + MoveIt Task Pipeline + BSD Michael Goerner + Robert Haschke catkin moveit_core + moveit_ros_planning + moveit_ros_planning_interface + moveit_msgs + rviz + moveit_core + moveit_msgs + rviz + rosunit + + + + diff --git a/src/demo/CMakeLists.txt b/src/demo/CMakeLists.txt index 94f8658d..d82dd601 100644 --- a/src/demo/CMakeLists.txt +++ b/src/demo/CMakeLists.txt @@ -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) diff --git a/src/introspection.cpp b/src/introspection.cpp index 30a9f1b8..87cfb52d 100644 --- a/src/introspection.cpp +++ b/src/introspection.cpp @@ -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(task_topic, 1); diff --git a/src/stage_p.h b/src/stage_p.h index b5c410d5..9c39ceef 100644 --- a/src/stage_p.h +++ b/src/stage_p.h @@ -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 InterfaceFlags; - - class ContainerBase; class StagePrivate { friend class Stage; diff --git a/src/stages/CMakeLists.txt b/src/stages/CMakeLists.txt index 398f11e7..e6271c3a 100644 --- a/src/stages/CMakeLists.txt +++ b/src/stages/CMakeLists.txt @@ -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}) diff --git a/src/task.cpp b/src/task.cpp index 91073ceb..a729f43a 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -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(); } diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 4a8f6566..e4dff25b 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -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) diff --git a/src/test/container.cpp b/src/test/test_container.cpp similarity index 98% rename from src/test/container.cpp rename to src/test/test_container.cpp index 659cff7f..9fc08c29 100644 --- a/src/test/container.cpp +++ b/src/test/test_container.cpp @@ -4,7 +4,7 @@ #include #include -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())); EXPECT_THROW(c.init(scene), InitStageException); } - -} } diff --git a/src/test/test_task_model.cpp b/src/test/test_task_model.cpp new file mode 100644 index 00000000..c389779a --- /dev/null +++ b/src/test/test_task_model.cpp @@ -0,0 +1,137 @@ +#include +#include +#include + +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 &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); +} diff --git a/task_panel_rviz_plugin_description.xml b/task_panel_rviz_plugin_description.xml new file mode 100644 index 00000000..82284577 --- /dev/null +++ b/task_panel_rviz_plugin_description.xml @@ -0,0 +1,9 @@ + + + + A panel widget to visualize and edit planning tasks + + + diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt new file mode 100644 index 00000000..e7437ec9 --- /dev/null +++ b/visualization/CMakeLists.txt @@ -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) diff --git a/visualization/task_panel/CMakeLists.txt b/visualization/task_panel/CMakeLists.txt new file mode 100644 index 00000000..7be033bb --- /dev/null +++ b/visualization/task_panel/CMakeLists.txt @@ -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}) diff --git a/visualization/task_panel/mainloop_processing.cpp b/visualization/task_panel/mainloop_processing.cpp new file mode 100644 index 00000000..8ed511ce --- /dev/null +++ b/visualization/task_panel/mainloop_processing.cpp @@ -0,0 +1,55 @@ +#include "mainloop_processing.h" +#include + +namespace moveit { namespace tools { + +MainLoopProcessing::MainLoopProcessing(QObject *parent) : QObject(parent) +{ +} + +void MainLoopProcessing::addJob(const std::function& job) +{ + boost::unique_lock ulock(jobs_mutex_); + jobs_.push_back(job); +} + +void MainLoopProcessing::clear() +{ + jobs_.clear(); +} + +size_t MainLoopProcessing::numPending() +{ + boost::unique_lock ulock(jobs_mutex_); + return jobs_.size(); +} + +void MainLoopProcessing::waitForAllJobs() +{ + boost::unique_lock ulock(jobs_mutex_); + while (!jobs_.empty()) + idle_condition_.wait(ulock); +} + +void MainLoopProcessing::executeJobs() +{ + boost::unique_lock ulock(jobs_mutex_); + while (!jobs_.empty()) + { + std::function 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(); +} + +} } diff --git a/visualization/task_panel/mainloop_processing.h b/visualization/task_panel/mainloop_processing.h new file mode 100644 index 00000000..16214d63 --- /dev/null +++ b/visualization/task_panel/mainloop_processing.h @@ -0,0 +1,29 @@ +#pragma once +#include + +#include +#include +#include +#include +#include + +namespace moveit { namespace tools { + +class MainLoopProcessing : public QObject +{ + Q_OBJECT + boost::mutex jobs_mutex_; + std::deque > jobs_; + boost::condition_variable idle_condition_; + +public: + explicit MainLoopProcessing(QObject *parent = 0); + void addJob(const std::function &job); + void clear(); + size_t numPending(); + + void waitForAllJobs(); + void executeJobs(); +}; + +} } diff --git a/visualization/task_panel/stage_wrapper.cpp b/visualization/task_panel/stage_wrapper.cpp new file mode 100644 index 00000000..d556504f --- /dev/null +++ b/visualization/task_panel/stage_wrapper.cpp @@ -0,0 +1,35 @@ +#include + +#include "stage_wrapper.h" +#include + +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; +} + +} diff --git a/visualization/task_panel/stage_wrapper.h b/visualization/task_panel/stage_wrapper.h new file mode 100644 index 00000000..605dff0d --- /dev/null +++ b/visualization/task_panel/stage_wrapper.h @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include +#include + +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 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> 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&& 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 id_to_stage; + +public: + RemoteTask() : RemoteStage(nullptr) {} +}; + +} diff --git a/visualization/task_panel/task_model.cpp b/visualization/task_panel/task_model.cpp new file mode 100644 index 00000000..0f3ed4c4 --- /dev/null +++ b/visualization/task_panel/task_model.cpp @@ -0,0 +1,248 @@ +#include "task_model.h" +#include "stage_wrapper.h" +#include +#include + +namespace moveit_rviz_plugin { + +class TaskModelPrivate { +public: + typedef std::vector> 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 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(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(remote_task)); + endInsertRows(); + } +} + +typedef moveit_task_constructor::Stage StageMsg; +static std::map +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(stage)); + if (notify) endInsertRows(); + } + } +} + +} diff --git a/visualization/task_panel/task_model.h b/visualization/task_panel/task_model.h new file mode 100644 index 00000000..18f4b7b6 --- /dev/null +++ b/visualization/task_panel/task_model.h @@ -0,0 +1,40 @@ +#pragma once + +#include +#include + +#include + +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); +}; + +} diff --git a/visualization/task_panel/task_panel.cpp b/visualization/task_panel/task_panel.cpp new file mode 100644 index 00000000..bababf16 --- /dev/null +++ b/visualization/task_panel/task_panel.cpp @@ -0,0 +1,115 @@ +#include + +#include "task_panel.h" +#include "ui_task_panel.h" +#include "task_model.h" + +#include +#include +#include +#include "mainloop_processing.h" + +#include +#include +#include +#include + +#include +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(), + "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_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz::Panel) diff --git a/visualization/task_panel/task_panel.h b/visualization/task_panel/task_panel.h new file mode 100644 index 00000000..67d16244 --- /dev/null +++ b/visualization/task_panel/task_panel.h @@ -0,0 +1,30 @@ +#pragma once +#include + +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()) +}; + +} diff --git a/visualization/task_panel/task_panel.ui b/visualization/task_panel/task_panel.ui new file mode 100644 index 00000000..f80a4144 --- /dev/null +++ b/visualization/task_panel/task_panel.ui @@ -0,0 +1,48 @@ + + + moveit_rviz_plugin::TaskPanel + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + Task Tree + + + + + + + + + + Settings + + + + + + + + + + + rviz::PropertyTreeWidget + QTreeView +
rviz/properties/property_tree_widget.h
+
+
+ + +