Skip to content
Snippets Groups Projects
Select Git revision
  • e7fc1098c349b1469c1eb9cd25afe9f1f68ad847
  • main default protected
  • mg2bt
  • Part1
4 results

test_reader.cpp

Blame
  • user avatar
    KingMaZito authored
    e7fc1098
    History
    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();
    }