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>
13 #include <boost/circular_buffer.hpp>
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;
18 boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
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))};
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});
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();
37 }
else {cb_jd.push_back(temp);}
40 ASSERT_EQ(cb_jd.size(), 0);
43 TEST(BtTestSuit, basicCooperativeTest){
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;
50 boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
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))};
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});
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();
71 }
else {cb_jd.push_back(temp);}
74 ASSERT_EQ(cb_jd.size(), 0);
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);
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))};
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});
94 bool jobs_remaining =
true;
95 while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
97 for (
auto it = cb_jd.begin(); it != cb_jd.end();) {
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();
107 it = cb_jd.erase(it);
111 ready_to_move =
false;
114 if (!ready_to_move) {
118 jobs_remaining = moved;
121 ASSERT_EQ(cb_jd.size(), 1);
123 cb_jd.push_back({
"panda_arm1", job3});
124 cb_jd.push_back({
"panda_arm1", job1});
125 cb_jd.push_back({
"panda_arm2", job2});
127 jobs_remaining =
true;
128 while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
130 for (
auto it = cb_jd.begin(); it != cb_jd.end();) {
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();
140 it = cb_jd.erase(it);
144 ready_to_move =
false;
147 if (!ready_to_move) {
151 jobs_remaining = moved;
154 ASSERT_EQ(cb_jd.size(), 3);
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)}}
164 std::vector<std::vector<std::string>> result;
165 std::set<std::string> keysUsed;
167 for (
const auto& pair1 : input_map) {
168 if (keysUsed.count(pair1.first) > 0)
continue;
170 auto lastVector1 = pair1.second.back();
171 std::vector<std::string> group = {pair1.first};
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();
177 if (lastVector1 == firstVector2) {
178 group.push_back(pair2.first);
179 lastVector1 = pair2.second.back();
180 keysUsed.insert(pair2.first);
183 result.push_back(group);
186 ASSERT_EQ(result.size(),2);
190 int main(
int argc,
char **argv){
191 testing::InitGoogleTest(&argc, argv);
192 ros::init(argc, argv,
"bt-test");
194 return RUN_ALL_TESTS();