Select Git revision
test_reader.cpp
test_reader.cpp 2.59 KiB
#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();
}