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

first few tests

parent f42315e1
No related branches found
No related tags found
No related merge requests found
...@@ -76,5 +76,9 @@ ...@@ -76,5 +76,9 @@
"typeinfo": "cpp", "typeinfo": "cpp",
"variant": "cpp", "variant": "cpp",
"future": "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
...@@ -30,6 +30,7 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -30,6 +30,7 @@ find_package(catkin REQUIRED COMPONENTS
behaviortree_cpp_v3 behaviortree_cpp_v3
) )
find_package(rostest REQUIRED)
find_package(yaml-cpp REQUIRED) find_package(yaml-cpp REQUIRED)
find_package(Protobuf 3 REQUIRED) find_package(Protobuf 3 REQUIRED)
include_directories(${CMAKE_CURRENT_BINARY_DIR}) include_directories(${CMAKE_CURRENT_BINARY_DIR})
...@@ -178,3 +179,26 @@ target_link_libraries(mtc2taskspace ...@@ -178,3 +179,26 @@ target_link_libraries(mtc2taskspace
${BEHAVIOR_TREE_LIBRARY} ${BEHAVIOR_TREE_LIBRARY}
yaml-cpp 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>
<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
#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
#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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment