test_task_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Bielefeld University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Bielefeld University nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Robert Haschke */
36 
37 #include <src/task_list_model.h>
38 #include <src/local_task_model.h>
39 #include <src/remote_task_model.h>
42 
43 #include <ros/init.h>
44 #include <gtest/gtest.h>
45 #include <initializer_list>
46 #include <qcoreapplication.h>
47 #include <qtimer.h>
48 
49 using namespace moveit::task_constructor;
50 
51 class TaskListModelTest : public ::testing::Test
52 {
53 protected:
56  int children = 0;
57  int num_inserts = 0;
58  int num_updates = 0;
59 
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;
63  uint id = 0, root_id;
64  t.task_id = task_id.empty() ? name : task_id;
65 
66  moveit_task_constructor_msgs::StageDescription desc;
67  desc.parent_id = id;
68  desc.id = root_id = ++id;
69  desc.name = name;
70  t.stages.push_back(desc);
71 
72  for (int i = 0; i != children; ++i) {
73  desc.parent_id = root_id;
74  desc.id = ++id;
75  desc.name = std::to_string(i);
76  t.stages.push_back(desc);
77  }
78  return t;
79  }
80 
81  void validate(QAbstractItemModel& model, const std::initializer_list<const char*>& expected) {
82  // validate root index
83  ASSERT_EQ(model.rowCount(), static_cast<int>(expected.size()));
84  EXPECT_EQ(model.columnCount(), 4);
85  EXPECT_EQ(model.parent(QModelIndex()), QModelIndex());
86 
87  // validate first-level items
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));
92  EXPECT_EQ(idx.column(), 0);
93  EXPECT_EQ(idx.model(), &model);
94  EXPECT_EQ(model.parent(idx), QModelIndex());
95 
96  // validate data
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);
100 
101  // validate children
102  ASSERT_EQ(model.rowCount(idx), children);
103  EXPECT_EQ(model.columnCount(idx), 4);
104  EXPECT_EQ(model.rowCount(model.index(row, 1)), 0);
105 
106  // validate second-level items
107  for (int child = 0; child < children; ++child) {
108  QModelIndex child_idx = model.index(child, 0, idx);
109  EXPECT_EQ(child_idx.row(), child);
110  EXPECT_EQ(child_idx.column(), 0);
111  EXPECT_EQ(child_idx.model(), &model);
112  EXPECT_EQ(model.parent(child_idx), idx);
113 
114  EXPECT_EQ(model.data(child_idx).toString().toStdString(), std::to_string(child));
115  EXPECT_EQ(model.rowCount(child_idx), 0);
116  EXPECT_EQ(model.columnCount(child_idx), 4);
117  }
118  }
119  }
120 
122  {
123  SCOPED_TRACE("empty");
124  validate(model, {});
125  }
126  EXPECT_EQ(num_inserts, 0);
127  EXPECT_EQ(num_updates, 0);
128 
129  for (int i = 0; i < 2; ++i) {
130  SCOPED_TRACE("first i=" + std::to_string(i));
131  num_inserts = 0;
132  num_updates = 0;
133  model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution");
134 
135  if (i == 0)
136  EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task
137  else
138  EXPECT_EQ(num_inserts, 0);
139  EXPECT_EQ(num_updates, 0);
140 
141  validate(model, { "first" });
142  }
143  for (int i = 0; i < 2; ++i) {
144  SCOPED_TRACE("second i=" + std::to_string(i));
145  num_inserts = 0;
146  num_updates = 0;
147  model.processTaskDescriptionMessage(genMsg("second"), nh, "get_solution"); // 1 notify for inserted task
148 
149  if (i == 0)
150  EXPECT_EQ(num_inserts, 1);
151  else
152  EXPECT_EQ(num_inserts, 0);
153  EXPECT_EQ(num_updates, 0);
154 
155  validate(model, { "first", "second" });
156  }
157  }
158 
159  void SetUp() override {
160  QObject::connect(&model, &QAbstractItemModel::rowsAboutToBeInserted, [this]() { ++num_inserts; });
161  QObject::connect(&model, &QAbstractItemModel::dataChanged, [this]() { ++num_updates; });
162  }
163  void TearDown() override {}
164 };
165 
166 TEST_F(TaskListModelTest, remoteTaskModel) {
167  children = 3;
168  planning_scene::PlanningSceneConstPtr scene;
169  moveit_rviz_plugin::RemoteTaskModel m(nh, "get_solution", scene, nullptr);
170  m.processStageDescriptions(genMsg("first").stages);
171  SCOPED_TRACE("first");
172  validate(m, { "first" });
173 }
174 
175 TEST_F(TaskListModelTest, localTaskModel) {
176  int argc = 0;
177  char* argv = nullptr;
178  ros::init(argc, &argv, "testLocalTaskModel");
179 
180  children = 3;
181  const char* task_name = "task pipeline";
182  moveit_rviz_plugin::LocalTaskModel m(std::make_unique<SerialContainer>(task_name),
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)));
186 
187  {
188  SCOPED_TRACE("localTaskModel");
189  validate(m, { task_name });
190  }
191 }
192 
193 TEST_F(TaskListModelTest, noChildren) {
194  children = 0;
195  populateAndValidate();
196 }
197 
198 TEST_F(TaskListModelTest, threeChildren) {
199  children = 3;
200  populateAndValidate();
201 }
202 
203 TEST_F(TaskListModelTest, visitedPopulate) {
204  // first population without children
205  children = 0;
206  model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution");
207  validate(model, { "first" }); // validation visits root node
208  EXPECT_EQ(num_inserts, 1);
209 
210  children = 3;
211  num_inserts = 0;
212  model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution");
213  validate(model, { "first" });
214  // second population with children should emit insert notifies for them
215  EXPECT_EQ(num_inserts, 3);
216  EXPECT_EQ(num_updates, 0);
217 }
218 
220  children = 3;
221  model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution");
222  auto m = model.getModel(model.index(0, 0)).first;
223  int num_deletes = 0;
224  QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; });
225 
226  model.removeModel(m);
227  // process deleteLater() events
228  QCoreApplication::sendPostedEvents(nullptr, QEvent::DeferredDelete);
229  // as m is owned by model, m should be destroyed
230  EXPECT_EQ(num_deletes, 1);
231  EXPECT_EQ(model.rowCount(), 0);
232 }
233 
234 int main(int argc, char** argv) {
235  ros::init(argc, argv, "test_task_model");
236  QCoreApplication app(argc, argv);
237  // https://bugs.llvm.org/show_bug.cgi?id=40367
238  // NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks)
239  QTimer::singleShot(0, [&]() {
240  ::testing::InitGoogleTest(&argc, argv);
241  auto test_result = RUN_ALL_TESTS();
242  app.exit(test_result);
243  });
244  return app.exec();
245 }
remote_task_model.h
TaskListModelTest
Definition: test_task_model.cpp:51
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
TEST_F
TEST_F(TaskListModelTest, remoteTaskModel)
Definition: test_task_model.cpp:166
TaskListModelTest::nh
ros::NodeHandle nh
Definition: test_task_model.cpp:54
init.h
TaskListModelTest::TearDown
void TearDown() override
Definition: test_task_model.cpp:163
validate
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
container.h
main
int main(int argc, char **argv)
Definition: test_task_model.cpp:234
current_state.h
moveit_rviz_plugin::TaskListModel
Definition: task_list_model.h:120
moveit::task_constructor
TaskListModelTest::validate
void validate(QAbstractItemModel &model, const std::initializer_list< const char * > &expected)
Definition: test_task_model.cpp:81
TaskListModelTest::model
moveit_rviz_plugin::TaskListModel model
Definition: test_task_model.cpp:55
moveit_rviz_plugin::utils::FlatMergeProxyModel::index
QModelIndex index(int row, int column, const QModelIndex &parent=QModelIndex()) const override
Definition: flat_merge_proxy_model.cpp:337
TaskListModelTest::genMsg
moveit_task_constructor_msgs::TaskDescription genMsg(const std::string &name, const std::string &task_id=std::string())
Definition: test_task_model.cpp:60
name
name
moveit_rviz_plugin::RemoteTaskModel
Definition: remote_task_model.h:84
moveit_rviz_plugin::utils::FlatMergeProxyModel::rowCount
int rowCount(const QModelIndex &parent=QModelIndex()) const override
Definition: flat_merge_proxy_model.cpp:313
moveit_rviz_plugin::TaskListModel::processTaskDescriptionMessage
void processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription &msg, ros::NodeHandle &nh, const std::string &service_name)
process an incoming task description message - only call in Qt's main loop
Definition: task_list_model.cpp:244
m
m
local_task_model.h
task_list_model.h
moveit_rviz_plugin::utils::FlatMergeProxyModel::removeModel
bool removeModel(QAbstractItemModel *model)
remove (first) matching model
Definition: flat_merge_proxy_model.cpp:528
moveit_rviz_plugin::utils::FlatMergeProxyModel::getModel
std::pair< QAbstractItemModel *, QModelIndex > getModel(const QModelIndex &index) const
retrieve model corresponding to given index
Definition: flat_merge_proxy_model.cpp:516
TaskListModelTest::SetUp
void SetUp() override
Definition: test_task_model.cpp:159
app
app
TaskListModelTest::populateAndValidate
void populateAndValidate()
Definition: test_task_model.cpp:121
t
dictionary t
moveit_rviz_plugin::LocalTaskModel
Definition: local_task_model.h:76
EXPECT_EQ
#define EXPECT_EQ(a, b)
ros::NodeHandle


visualization
Author(s): Robert Haschke
autogenerated on Thu Feb 27 2025 03:39:51