Skip to content
Snippets Groups Projects
Commit ae0e0eb7 authored by KingMaZito's avatar KingMaZito
Browse files

evaluation

parent e7fc1098
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
test/reader/test_bt.cpp
+ 159
− 10
View file @ ae0e0eb7
......@@ -13,11 +13,6 @@
#include <boost/circular_buffer.hpp>
TEST(BtTestSuit, basicParallelTest){
BT::BehaviorTreeFactory factory;
factory.registerNodeType<Execution>("Execution");
factory.registerNodeType<PositionCondition>("PositionCondition");
factory.registerNodeType<Parallel_robot>("Parallel_robot");
std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
......@@ -26,17 +21,17 @@ TEST(BtTestSuit, basicParallelTest){
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))};
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))};
std::vector<tf2::Vector3> boxes = {tf2::Vector3(0,0,0), tf2::Vector3(2,2,0)};
cb_jd.push_back({"box1", job1});
cb_jd.push_back({"box2", job2});
std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
cb_jd.push_back({"panda_arm1", job1});
cb_jd.push_back({"panda_arm2", job2});
while(!cb_jd.empty()){
for(auto& box : boxes){
std::pair<std::string, job_data> temp = cb_jd.front();
if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box) == 0) {
if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box.second) == 0) {
for (int k = 1; k < temp.second.jobs_.size(); k++){
box = temp.second.jobs_[k].getOrigin();
box.second = temp.second.jobs_[k].getOrigin();
}
cb_jd.pop_front();
} else {cb_jd.push_back(temp);}
......@@ -45,6 +40,160 @@ TEST(BtTestSuit, basicParallelTest){
ASSERT_EQ(cb_jd.size(), 0); //All jobs
}
TEST(BtTestSuit, basicCooperativeTest){
// BT::BehaviorTreeFactory factory;
// factory.registerNodeType<Execution>("Execution");
// factory.registerNodeType<PositionCondition>("PositionCondition");
// factory.registerNodeType<Parallel_robot>("Parallel_robot");
std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
boost::circular_buffer<std::pair<std::string, job_data>> cb_jd;
job_data job1, job2, job3;
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))};
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))};
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))};
std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
cb_jd.push_back({"panda_arm1", job1});
cb_jd.push_back({"panda_arm2", job2});
cb_jd.push_back({"panda_arm1", job3});
while(!cb_jd.empty()){
for(auto& box : boxes){
std::pair<std::string, job_data> temp = cb_jd.front();
if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), box.second) == 0) {
for (int k = 1; k < temp.second.jobs_.size(); k++){
box.second = temp.second.jobs_[k].getOrigin();
}
cb_jd.pop_front();
} else {cb_jd.push_back(temp);}
}
}
ASSERT_EQ(cb_jd.size(), 0); //All jobs
}
TEST(BtTestSuit, ComplexCooperativeTest){
std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
boost::circular_buffer<std::pair<std::string, job_data>> cb_jd(3);
job_data job1, job2, job3;
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))};
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))};
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))};
std::vector<std::pair<std::string,tf2::Vector3>> boxes = {{"box1", tf2::Vector3(0,0,0)}, {"box2", tf2::Vector3(2,2,0)}};
cb_jd.push_back({"panda_arm1", job3});
cb_jd.push_back({"panda_arm1", job1});
cb_jd.push_back({"panda_arm2", job2});
//ASSERT_FALSE(boxes[0].second == cb_jd[0].second.jobs_[0].getOrigin());
bool jobs_remaining = true;
while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
bool moved = false;
for (auto it = cb_jd.begin(); it != cb_jd.end();) {
auto& temp = *it;
bool ready_to_move = true;
for (auto& box : boxes) {
if (temp.second.jobs_.front().getOrigin() == box.second) {
for (int k = 1; k < temp.second.jobs_.size(); k++) {
box.second = temp.second.jobs_[k].getOrigin();
}
if (ready_to_move) {
moved = true;
it = cb_jd.erase(it);
break;
}
} else {
ready_to_move = false;
}
}
if (!ready_to_move) {
++it;
}
}
jobs_remaining = moved;
}
ASSERT_EQ(cb_jd.size(), 1); // All jobs
boxes.clear();
cb_jd.push_back({"panda_arm1", job3});
cb_jd.push_back({"panda_arm1", job1});
cb_jd.push_back({"panda_arm2", job2});
jobs_remaining = true;
while (jobs_remaining && !cb_jd.empty() && !boxes.empty()) {
bool moved = false;
for (auto it = cb_jd.begin(); it != cb_jd.end();) {
auto& temp = *it;
bool ready_to_move = true;
for (auto& box : boxes) {
if (temp.second.jobs_.front().getOrigin() == box.second) {
for (int k = 1; k < temp.second.jobs_.size(); k++) {
box.second = temp.second.jobs_[k].getOrigin();
}
if (ready_to_move) {
moved = true;
it = cb_jd.erase(it);
break;
}
} else {
ready_to_move = false;
}
}
if (!ready_to_move) {
++it;
}
}
jobs_remaining = moved;
}
ASSERT_EQ(cb_jd.size(), 3); // All jobs
}
TEST(BtTestSuit, bahviorTest){
//std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_ = std::make_unique<moveit::planning_interface::PlanningSceneInterface>();
BT::BehaviorTreeFactory factory;
factory.registerNodeType<Execution>("Execution");
factory.registerNodeType<PositionCondition>("PositionCondition");
factory.registerNodeType<Parallel_robot>("Parallel_robot");
std::vector<std::string> robots_ = {"panda_arm1", "panda_arm2"};
std::map<std::string, std::vector<std::tuple<std::string, tf2::Vector3, std::vector<tf2::Vector3>>>> tasks;
tasks["panda_arm1"].push_back({"box1", tf2::Vector3(0,0,0), {tf2::Vector3(0,0,0)}});
tasks["panda_arm2"].push_back({"box2", tf2::Vector3(0,0,0), {tf2::Vector3(0,0,0)}});
std::stringstream ss;
ss << "<root main_tree_to_execute = \"MainTree\">\n";
ss << "<BehaviorTree ID=\"MainTree\">\n";
ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\"2\" failure_threshold=\"2\">\n";
for (auto& rob : robots_){
ss << "<Control ID=\"Parallel_robot\" name=\""<< rob <<"\" success_threshold=\"3\" failure_threshold=\"3\">\n";
for(auto& task : tasks){
ss << "<SequenceStar name=\"root_sequence\">\n";
ss << "<Condition ID=\"PositionCondition\" name=\"Position_condition\"/>\n";
ss << "<Action ID=\"Execution\" name=\"Execution\"/>\n";
ss << "</SequenceStar>\n";
}
ss << "</Control>\n";
}
ss << "</Control>\n";
ss << "</BehaviorTree>\n";
ss << "</root>\n";
auto tree = factory.createTreeFromText(ss.str());
auto node_it = tree.nodes.begin();
ASSERT_TRUE(true);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "bt-test");
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment