diff --git a/.vscode/settings.json b/.vscode/settings.json
index 1de818d0562b9e77025cae6af75d7bd8ffe63c48..3b5e761a83812a653f8ddec23279ee9bf2f6671f 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -76,5 +76,9 @@
"typeinfo": "cpp",
"variant": "cpp",
"future": "cpp"
- }
+ },
+ "python.analysis.extraPaths": [
+ "/home/matteo/ccf_sim_workspace/devel/lib/python3/dist-packages",
+ "/opt/ros/noetic/lib/python3/dist-packages"
+ ]
}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd6008a397cc6d026cc3410c6d97780a81f37c24..496e8f3b10a382db1a404ec30e99e479a8ae0b3e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,6 +30,7 @@ find_package(catkin REQUIRED COMPONENTS
behaviortree_cpp_v3
)
+find_package(rostest REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(Protobuf 3 REQUIRED)
include_directories(${CMAKE_CURRENT_BINARY_DIR})
@@ -178,3 +179,26 @@ target_link_libraries(mtc2taskspace
${BEHAVIOR_TREE_LIBRARY}
yaml-cpp
)
+
+
+#### TEST ####
+
+catkin_add_gtest(reader-test launch/integration.test test/reader/test_reader.cpp
+src/reader/abstract_param_reader.cpp
+src/reader/cuboid_reader.cpp
+src/reader/job_reader.cpp
+src/reader/map_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/task_space_reader.cpp
+src/reader/ts_reader.cpp
+src/reader/wing_reader.cpp
+)
+
+catkin_add_gtest(bt-test launch/integration.test test/reader/test_bt.cpp
+src/reader/abstract_param_reader.cpp
+src/bt/execution.cpp
+src/bt/position_condition.cpp
+src/bt/parallel_robot.cpp
+)
+target_link_libraries(reader-test ${catkin_LIBRARIES} yaml-cpp)
+target_link_libraries(bt-test ${catkin_LIBRARIES} ${BEHAVIOR_TREE_LIBRARY} yaml-cpp)
\ No newline at end of file
diff --git a/launch/integration.test b/launch/integration.test
new file mode 100644
index 0000000000000000000000000000000000000000..24d10f08c29826ce4850cb987e320fa3dfa890bd
--- /dev/null
+++ b/launch/integration.test
@@ -0,0 +1,14 @@
+<launch>
+ <arg name="map" default="dummy.yaml" />
+ <arg name="jobs" default="jobs/dummy.yaml" />
+ <arg name="resource" default="dummy" />
+
+
+ <rosparam command="load" file="$(find multi_cell_builder)/maps/$(arg map)"/>
+ <rosparam command="load" file="$(find multi_cell_builder)/resources/$(arg resource).yaml"/>
+ <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
+
+ <test test-name="Reader-Tests" pkg="multi_cell_builder" type="reader-test" name="Readers"/>
+ <test test-name="Bt-Tests" pkg="multi_cell_builder" type="bt-test" name="BehaviotTree"/>
+
+</launch>
\ No newline at end of file
diff --git a/test/reader/test_bt.cpp b/test/reader/test_bt.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3635ab4eb927d1745cf782324ebd0b264f4b4c18
--- /dev/null
+++ b/test/reader/test_bt.cpp
@@ -0,0 +1,53 @@
+#include <ros/ros.h>
+#include <behaviortree_cpp_v3/behavior_tree.h>
+#include <behaviortree_cpp_v3/tree_node.h>
+#include <behaviortree_cpp_v3/bt_factory.h>
+#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>
+#include <stdint.h>
+
+#include "reader/abstract_param_reader.h"
+#include "bt/execution.h"
+#include "bt/position_condition.h"
+#include "bt/parallel_robot.h"
+
+#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;
+ job_data job1;
+ job_data job2;
+ 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});
+
+
+ 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) {
+ for (int k = 1; k < temp.second.jobs_.size(); k++){
+ box = temp.second.jobs_[k].getOrigin();
+ }
+ cb_jd.pop_front();
+ } else {cb_jd.push_back(temp);}
+ }
+ }
+ ASSERT_EQ(cb_jd.size(), 0); //All jobs
+}
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "bt-test");
+ ros::NodeHandle nh;
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/reader/test_reader.cpp b/test/reader/test_reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..306167664fced3af5d7430dd591074435ebe381a
--- /dev/null
+++ b/test/reader/test_reader.cpp
@@ -0,0 +1,72 @@
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+#include "reader/cuboid_reader.h"
+#include "reader/job_reader.h"
+#include "reader/map_reader.h"
+#include "reader/robot_reader.h"
+
+
+TEST(ReaderTestSuit, cuboidReader){
+ std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
+ std::unique_ptr<CuboidReader> cr = std::make_unique<CuboidReader>(nh);
+ ASSERT_EQ(cr->cuboidBin().size(), 7);
+ ASSERT_EQ(cr->cuboidObstacle().size(), 0);
+}
+
+TEST(ReaderTestSuit, jobReader){
+ std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
+ std::unique_ptr<JobReader> jr = std::make_unique<JobReader>(nh);
+
+ // Check jobs per agent
+ int jobs1 = 0;
+ int jobs2 = 0;
+ for (auto& jobs: jr->robotData()){
+ if(jobs.first == "panda_arm1"){
+ jobs1++;
+ }
+ if(jobs.first == "panda_arm1"){
+ jobs2++;
+ }
+ }
+ ASSERT_EQ(jobs1, 1);
+ ASSERT_EQ(jobs2, 1);
+ ASSERT_EQ(jobs1 + jobs2, 2); //All jobs
+}
+
+TEST(ReaderTestSuit, mapReader){
+ std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
+ std::unique_ptr<MapReader> mr = std::make_unique<MapReader>(nh);
+
+ // count entrys
+ ASSERT_EQ(mr->mapData().size(), 6864);
+}
+
+TEST(ReaderTestSuit, robotReader){
+ std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
+ std::unique_ptr<RobotReader> rr = std::make_unique<RobotReader>(nh);
+
+ // count entrys
+ ASSERT_EQ(rr->robotData().size(), 2);
+ ASSERT_EQ(rr->robotData()[0].name_, "panda_arm1");
+ const tf2::Vector3 position1(0,0,0.885f/2.0f);
+ const tf2::Vector3 size1(0.8f/2.0f, 0.8f/2.0f,position1.getZ());
+ const tf2::Quaternion rot1(0,0, 0, 1);
+ ASSERT_EQ(static_cast<int>(rr->robotData()[0].pose_.getOrigin().distance2(position1)), 0);
+ ASSERT_EQ(static_cast<int>(rr->robotData()[0].size_.distance2(size1)), 0);
+ ASSERT_EQ(static_cast<int>(rr->robotData()[0].pose_.getRotation().length2() - rot1.length2()), 0);
+
+ ASSERT_EQ(rr->robotData()[1].name_, "panda_arm2");
+ const tf2::Vector3 position2(0,1.31f,0.885f/2.0f);
+ const tf2::Vector3 size2(0.8f/2.0f, 0.8f/2.0f,position2.getZ());
+ const tf2::Quaternion rot2(0,0, 0, 1);
+ ASSERT_EQ(static_cast<int>(rr->robotData()[1].pose_.getOrigin().distance2(position2)), 0);
+ ASSERT_EQ(static_cast<int>(rr->robotData()[1].size_.distance2(size2)), 0);
+ ASSERT_EQ(static_cast<int>(rr->robotData()[1].pose_.getRotation().length2() - rot2.length2()), 0);
+}
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "reader-test");
+ ros::NodeHandle nh;
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file