44 #include <gtest/gtest.h>
45 #include <initializer_list>
46 #include <qcoreapplication.h>
60 moveit_task_constructor_msgs::TaskDescription
genMsg(
const std::string& name,
61 const std::string& task_id = std::string()) {
62 moveit_task_constructor_msgs::TaskDescription
t;
64 t.task_id = task_id.empty() ?
name : task_id;
66 moveit_task_constructor_msgs::StageDescription desc;
68 desc.id = root_id = ++id;
70 t.stages.push_back(desc);
72 for (
int i = 0; i != children; ++i) {
73 desc.parent_id = root_id;
75 desc.name = std::to_string(i);
76 t.stages.push_back(desc);
81 void validate(QAbstractItemModel& model,
const std::initializer_list<const char*>& expected) {
83 ASSERT_EQ(model.rowCount(),
static_cast<int>(expected.size()));
85 EXPECT_EQ(model.parent(QModelIndex()), QModelIndex());
88 auto it = expected.begin();
89 for (
size_t row = 0; row < expected.size(); ++row, ++it) {
90 QModelIndex idx = model.index(row, 0);
91 EXPECT_EQ(idx.row(),
static_cast<int>(row));
94 EXPECT_EQ(model.parent(idx), QModelIndex());
97 EXPECT_STREQ(model.data(idx).toByteArray().constData(), *it);
98 EXPECT_EQ(model.data(model.index(row, 1)).toUInt(), 0u);
99 EXPECT_EQ(model.data(model.index(row, 2)).toUInt(), 0u);
102 ASSERT_EQ(model.rowCount(idx), children);
104 EXPECT_EQ(model.rowCount(model.index(row, 1)), 0);
107 for (
int child = 0; child < children; ++child) {
108 QModelIndex child_idx = model.index(child, 0, idx);
114 EXPECT_EQ(model.data(child_idx).toString().toStdString(), std::to_string(child));
116 EXPECT_EQ(model.columnCount(child_idx), 4);
123 SCOPED_TRACE(
"empty");
129 for (
int i = 0; i < 2; ++i) {
130 SCOPED_TRACE(
"first i=" + std::to_string(i));
143 for (
int i = 0; i < 2; ++i) {
144 SCOPED_TRACE(
"second i=" + std::to_string(i));
155 validate(model, {
"first",
"second" });
160 QObject::connect(&model, &QAbstractItemModel::rowsAboutToBeInserted, [
this]() { ++num_inserts; });
161 QObject::connect(&model, &QAbstractItemModel::dataChanged, [
this]() { ++num_updates; });
168 planning_scene::PlanningSceneConstPtr scene;
170 m.processStageDescriptions(genMsg(
"first").stages);
171 SCOPED_TRACE(
"first");
177 char* argv =
nullptr;
178 ros::init(argc, &argv,
"testLocalTaskModel");
181 const char* task_name =
"task pipeline";
183 planning_scene::PlanningSceneConstPtr(),
nullptr);
184 for (
int i = 0; i != children; ++i)
185 m.add(std::make_unique<stages::CurrentState>(std::to_string(i)));
188 SCOPED_TRACE(
"localTaskModel");
195 populateAndValidate();
200 populateAndValidate();
224 QObject::connect(
m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; });
228 QCoreApplication::sendPostedEvents(
nullptr, QEvent::DeferredDelete);
234 int main(
int argc,
char** argv) {
235 ros::init(argc, argv,
"test_task_model");
236 QCoreApplication
app(argc, argv);
239 QTimer::singleShot(0, [&]() {
240 ::testing::InitGoogleTest(&argc, argv);
241 auto test_result = RUN_ALL_TESTS();
242 app.exit(test_result);