first few tests

.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

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

launch/integration.test

+<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

test/reader/test_bt.cpp

+#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

test/reader/test_reader.cpp

+#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