test_reader.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <gtest/gtest.h>
3 #include "reader/cuboid_reader.h"
4 #include "reader/job_reader.h"
5 #include "reader/map_reader.h"
6 #include "reader/robot_reader.h"
7 
8 
9 TEST(ReaderTestSuit, cuboidReader){
10  std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
11  std::unique_ptr<CuboidReader> cr = std::make_unique<CuboidReader>(nh);
12  ASSERT_EQ(cr->cuboidBin().size(), 7);
13  ASSERT_EQ(cr->cuboidObstacle().size(), 0);
14 }
15 
16 TEST(ReaderTestSuit, jobReader){
17  std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
18  std::unique_ptr<JobReader> jr = std::make_unique<JobReader>(nh);
19 
20  // Check jobs per agent
21  int jobs1 = 0;
22  int jobs2 = 0;
23  for (auto& jobs: jr->robotData()){
24  if(jobs.first == "panda_arm1"){
25  jobs1++;
26  }
27  if(jobs.first == "panda_arm1"){
28  jobs2++;
29  }
30  }
31  ASSERT_EQ(jobs1, 1);
32  ASSERT_EQ(jobs2, 1);
33  ASSERT_EQ(jobs1 + jobs2, 2); //All jobs
34 }
35 
36 TEST(ReaderTestSuit, mapReader){
37  std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
38  std::unique_ptr<MapReader> mr = std::make_unique<MapReader>(nh);
39 
40  // count entrys
41  ASSERT_EQ(mr->mapData().size(), 6864);
42 }
43 
44 TEST(ReaderTestSuit, robotReader){
45  std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
46  std::unique_ptr<RobotReader> rr = std::make_unique<RobotReader>(nh);
47 
48  // count entrys
49  ASSERT_EQ(rr->robotData().size(), 2);
50  ASSERT_EQ(rr->robotData()[0].name_, "panda_arm1");
51  const tf2::Vector3 position1(0,0,0.885f/2.0f);
52  const tf2::Vector3 size1(0.8f/2.0f, 0.8f/2.0f,position1.getZ());
53  const tf2::Quaternion rot1(0,0, 0, 1);
54  ASSERT_EQ(static_cast<int>(rr->robotData()[0].pose_.getOrigin().distance2(position1)), 0);
55  ASSERT_EQ(static_cast<int>(rr->robotData()[0].size_.distance2(size1)), 0);
56  ASSERT_EQ(static_cast<int>(rr->robotData()[0].pose_.getRotation().length2() - rot1.length2()), 0);
57 
58  ASSERT_EQ(rr->robotData()[1].name_, "panda_arm2");
59  const tf2::Vector3 position2(0,1.31f,0.885f/2.0f);
60  const tf2::Vector3 size2(0.8f/2.0f, 0.8f/2.0f,position2.getZ());
61  const tf2::Quaternion rot2(0,0, 0, 1);
62  ASSERT_EQ(static_cast<int>(rr->robotData()[1].pose_.getOrigin().distance2(position2)), 0);
63  ASSERT_EQ(static_cast<int>(rr->robotData()[1].size_.distance2(size2)), 0);
64  ASSERT_EQ(static_cast<int>(rr->robotData()[1].pose_.getRotation().length2() - rot2.length2()), 0);
65 }
66 
67 int main(int argc, char **argv){
68  testing::InitGoogleTest(&argc, argv);
69  ros::init(argc, argv, "reader-test");
70  ros::NodeHandle nh;
71  return RUN_ALL_TESTS();
72 }
cuboid_reader.h
map_reader.h
job_reader.h
robot_reader.h
main
int main(int argc, char **argv)
Definition: test_reader.cpp:67
TEST
TEST(ReaderTestSuit, cuboidReader)
Definition: test_reader.cpp:9


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51