mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
TaskPanel: listen to task_monitoring topic
This commit is contained in:
parent
abe61ef9eb
commit
8a461af97c
@ -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)
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
16
package.xml
16
package.xml
@ -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>
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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})
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
} }
|
||||
137
src/test/test_task_model.cpp
Normal file
137
src/test/test_task_model.cpp
Normal 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);
|
||||
}
|
||||
9
task_panel_rviz_plugin_description.xml
Normal file
9
task_panel_rviz_plugin_description.xml
Normal 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>
|
||||
23
visualization/CMakeLists.txt
Normal file
23
visualization/CMakeLists.txt
Normal 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)
|
||||
20
visualization/task_panel/CMakeLists.txt
Normal file
20
visualization/task_panel/CMakeLists.txt
Normal 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})
|
||||
55
visualization/task_panel/mainloop_processing.cpp
Normal file
55
visualization/task_panel/mainloop_processing.cpp
Normal 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();
|
||||
}
|
||||
|
||||
} }
|
||||
29
visualization/task_panel/mainloop_processing.h
Normal file
29
visualization/task_panel/mainloop_processing.h
Normal 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();
|
||||
};
|
||||
|
||||
} }
|
||||
35
visualization/task_panel/stage_wrapper.cpp
Normal file
35
visualization/task_panel/stage_wrapper.cpp
Normal 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;
|
||||
}
|
||||
|
||||
}
|
||||
70
visualization/task_panel/stage_wrapper.h
Normal file
70
visualization/task_panel/stage_wrapper.h
Normal 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) {}
|
||||
};
|
||||
|
||||
}
|
||||
248
visualization/task_panel/task_model.cpp
Normal file
248
visualization/task_panel/task_model.cpp
Normal 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
40
visualization/task_panel/task_model.h
Normal file
40
visualization/task_panel/task_model.h
Normal 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);
|
||||
};
|
||||
|
||||
}
|
||||
115
visualization/task_panel/task_panel.cpp
Normal file
115
visualization/task_panel/task_panel.cpp
Normal 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)
|
||||
30
visualization/task_panel/task_panel.h
Normal file
30
visualization/task_panel/task_panel.h
Normal 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())
|
||||
};
|
||||
|
||||
}
|
||||
48
visualization/task_panel/task_panel.ui
Normal file
48
visualization/task_panel/task_panel.ui
Normal 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>
|
||||
Loading…
Reference in New Issue
Block a user