2 #include <gtest/gtest.h>
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);
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);
23 for (
auto& jobs: jr->robotData()){
24 if(jobs.first ==
"panda_arm1"){
27 if(jobs.first ==
"panda_arm1"){
33 ASSERT_EQ(jobs1 + jobs2, 2);
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);
41 ASSERT_EQ(mr->mapData().size(), 6864);
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);
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);
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);
67 int main(
int argc,
char **argv){
68 testing::InitGoogleTest(&argc, argv);
69 ros::init(argc, argv,
"reader-test");
71 return RUN_ALL_TESTS();