test_bt.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <behaviortree_cpp_v3/behavior_tree.h>
3 #include <behaviortree_cpp_v3/tree_node.h>
4 #include <behaviortree_cpp_v3/bt_factory.h>
5 #include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>
6 #include <stdint.h>
7 
9 #include "bt/execution.h"
10 #include "bt/position_condition.h"
11 #include "bt/parallel_robot.h"
12 
13 #include <boost/circular_buffer.hpp>
14 
15 TEST(BtTestSuit, basicParallelTest){
16  std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
17 
18  boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
19  job_data job1;
20  job_data job2;
21  job1.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,0,0))};
22  job2.jobs_ = {tf2::Transform(tf2::Quaternion(2,2,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,2,0))};
23 
24  std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
25  cb_jd.push_back({"panda_arm1", job1});
26  cb_jd.push_back({"panda_arm2", job2});
27 
28 
29  while(!cb_jd.empty()){
30  for(auto& box : boxes){
31  std::pair<std::string, job_data> temp = cb_jd.front();
32  if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box.second) == 0) {
33  for (int k = 1; k < temp.second.jobs_.size(); k++){
34  box.second = temp.second.jobs_[k].getOrigin();
35  }
36  cb_jd.pop_front();
37  } else {cb_jd.push_back(temp);}
38  }
39  }
40  ASSERT_EQ(cb_jd.size(), 0); //All jobs
41 }
42 
43 TEST(BtTestSuit, basicCooperativeTest){
44  // BT::BehaviorTreeFactory factory;
45  // factory.registerNodeType<Execution>("Execution");
46  // factory.registerNodeType<PositionCondition>("PositionCondition");
47  // factory.registerNodeType<Parallel_robot>("Parallel_robot");
48  std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
49 
50  boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
51  job_data job1, job2, job3;
52  job1.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,0,0))};
53  job2.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,2,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(2,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,3,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(3,2,0))};
54  job3.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(1,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0))};
55 
56 
57  std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
58  cb_jd.push_back({"panda_arm1", job1});
59  cb_jd.push_back({"panda_arm2", job2});
60  cb_jd.push_back({"panda_arm1", job3});
61 
62 
63  while(!cb_jd.empty()){
64  for(auto& box : boxes){
65  std::pair<std::string, job_data> temp = cb_jd.front();
66  if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box.second) == 0) {
67  for (int k = 1; k < temp.second.jobs_.size(); k++){
68  box.second = temp.second.jobs_[k].getOrigin();
69  }
70  cb_jd.pop_front();
71  } else {cb_jd.push_back(temp);}
72  }
73  }
74  ASSERT_EQ(cb_jd.size(), 0); //All jobs
75 }
76 
77 TEST(BtTestSuit, ComplexCooperativeTest){
78  std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
79  boost::circular_buffer<std::pair<std::string, job_data>> cb_jd(3);
80  job_data job1, job2, job3;
81  job1.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,2,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,2.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,3,0))};
82  job2.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,0.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1.5f,0))};
83  job3.jobs_ = {tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,1.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,2,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,2.5f,0)), tf2::Transform(tf2::Quaternion(0,0,0,1),tf2::Vector3(0,3,0))};
84 
85 
86  std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
87  cb_jd.push_back({"panda_arm1", job3});
88  cb_jd.push_back({"panda_arm1", job1});
89  cb_jd.push_back({"panda_arm2", job2});
90 
91  //ASSERT_FALSE(boxes[0].second == cb_jd[0].second.jobs_[0].getOrigin());
92 
93 
94  bool jobs_remaining = true;
95  while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
96  bool moved = false;
97  for (auto it = cb_jd.begin(); it != cb_jd.end();) {
98  auto& temp = *it;
99  bool ready_to_move = true;
100  for (auto& box : boxes) {
101  if (temp.second.jobs_.front().getOrigin() == box.second) {
102  for (int k = 1; k < temp.second.jobs_.size(); k++) {
103  box.second = temp.second.jobs_[k].getOrigin();
104  }
105  if (ready_to_move) {
106  moved = true;
107  it = cb_jd.erase(it);
108  break;
109  }
110  } else {
111  ready_to_move = false;
112  }
113  }
114  if (!ready_to_move) {
115  ++it;
116  }
117  }
118  jobs_remaining = moved;
119  }
120 
121  ASSERT_EQ(cb_jd.size(), 1); // All jobs
122  boxes.clear();
123  cb_jd.push_back({"panda_arm1", job3});
124  cb_jd.push_back({"panda_arm1", job1});
125  cb_jd.push_back({"panda_arm2", job2});
126 
127  jobs_remaining = true;
128  while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
129  bool moved = false;
130  for (auto it = cb_jd.begin(); it != cb_jd.end();) {
131  auto& temp = *it;
132  bool ready_to_move = true;
133  for (auto& box : boxes) {
134  if (temp.second.jobs_.front().getOrigin() == box.second) {
135  for (int k = 1; k < temp.second.jobs_.size(); k++) {
136  box.second = temp.second.jobs_[k].getOrigin();
137  }
138  if (ready_to_move) {
139  moved = true;
140  it = cb_jd.erase(it);
141  break;
142  }
143  } else {
144  ready_to_move = false;
145  }
146  }
147  if (!ready_to_move) {
148  ++it;
149  }
150  }
151  jobs_remaining = moved;
152  }
153 
154  ASSERT_EQ(cb_jd.size(), 3); // All jobs
155 }
156 
157 TEST(BaseTestSuit, stringTest){
158  std::map<std::string, std::vector<tf2::Vector3>> input_map = {
159  {"a", {tf2::Vector3(0,0,0), tf2::Vector3(1,0,0)}},
160  {"b", {tf2::Vector3(1,0,0), tf2::Vector3(2,0,0)}},
161  {"c", {tf2::Vector3(0,1,0), tf2::Vector3(0,2,0)}}
162  };
163 
164  std::vector<std::vector<std::string>> result;
165  std::set<std::string> keysUsed;
166 
167  for (const auto& pair1 : input_map) {
168  if (keysUsed.count(pair1.first) > 0) continue;
169 
170  auto lastVector1 = pair1.second.back();
171  std::vector<std::string> group = {pair1.first};
172 
173  for (const auto& pair2 : input_map) {
174  if (pair1.first == pair2.first || keysUsed.count(pair2.first) > 0) continue;
175  auto firstVector2 = pair2.second.front();
176 
177  if (lastVector1 == firstVector2) {
178  group.push_back(pair2.first);
179  lastVector1 = pair2.second.back();
180  keysUsed.insert(pair2.first);
181  }
182  }
183  result.push_back(group);
184  }
185 
186  ASSERT_EQ(result.size(),2);
187 }
188 
189 
190 int main(int argc, char **argv){
191  testing::InitGoogleTest(&argc, argv);
192  ros::init(argc, argv, "bt-test");
193  ros::NodeHandle nh;
194  return RUN_ALL_TESTS();
195 }
parallel_robot.h
job_data::jobs_
std::vector< tf2::Transform > jobs_
Definition: abstract_param_reader.h:22
abstract_param_reader.h
main
int main(int argc, char **argv)
Definition: test_bt.cpp:190
TEST
TEST(BtTestSuit, basicParallelTest)
Definition: test_bt.cpp:15
job_data
Job data.
Definition: abstract_param_reader.h:21
position_condition.h
execution.h


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51