test_primary_parser.cpp
Go to the documentation of this file.
1 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2 
3 // -- BEGIN LICENSE BLOCK ----------------------------------------------
4 // Copyright 2020 FZI Forschungszentrum Informatik
5 //
6 // Licensed under the Apache License, Version 2.0 (the "License");
7 // you may not use this file except in compliance with the License.
8 // You may obtain a copy of the License at
9 //
10 // http://www.apache.org/licenses/LICENSE-2.0
11 //
12 // Unless required by applicable law or agreed to in writing, software
13 // distributed under the License is distributed on an "AS IS" BASIS,
14 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 // See the License for the specific language governing permissions and
16 // limitations under the License.
17 // -- END LICENSE BLOCK ------------------------------------------------
18 
19 //----------------------------------------------------------------------
26 //----------------------------------------------------------------------
27 
28 #include <gtest/gtest.h>
29 
30 #include <ur_client_library/comm/bin_parser.h>
31 #include <ur_client_library/primary/primary_parser.h>
32 
33 using namespace urcl;
34 
35 TEST(primary_parser, parse_calibration_data)
36 {
37  /* First RobotState of UR5e from URSim v5.8
38  *
39  * This package contains:
40  * - ROBOT_MODE_DATA
41  * - JOINT_DATA
42  * - CARTESIAN_INFO
43  * - KINEMATICS_INFO
44  * - NEEDED_FOR_CALIB_DATA
45  * - MASTERBOARD_DATA
46  * - TOOL_DATA
47  * - CONFIGURATION_DATA
48  * - FORCE_MODE_DATA
49  * - ADDITIONAL_INFO
50  * - SAFETY_DATA
51  * - TOOL_COMM_INFO
52  * - TOOL_MODE_INFO
53  */
54  unsigned char raw_data[] = {
55  0x00, 0x00, 0x05, 0x6a, 0x10, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x05, 0xf8, 0x7d, 0x17, 0x40, 0x01,
56  0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x3f, 0xef, 0x0a, 0x3d, 0x70, 0xa3, 0xd7, 0x0a, 0x00, 0x00, 0x00,
57  0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb, 0x01,
58  0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0x00, 0x00, 0x00,
59  0x00, 0x00, 0x00, 0x00, 0x00, 0xa3, 0xa8, 0x79, 0x38, 0x00, 0x00, 0x00, 0x00, 0x41, 0xcc, 0x00, 0x00, 0x00, 0x00,
60  0x00, 0x00, 0xfd, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0,
61  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x09, 0x06, 0x5a, 0x00, 0x00, 0x00, 0x00, 0x41, 0xc8, 0x00,
62  0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xc0, 0x01, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x38, 0xc0, 0x01, 0x9f, 0xbe, 0x76,
63  0xc8, 0xb4, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xbf, 0x0f, 0xf7, 0x00, 0x00, 0x00, 0x00,
64  0x41, 0xc4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0xe9, 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0xbf, 0xe9,
65  0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x6e, 0xbb, 0xe2, 0x00,
66  0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97,
67  0xf6, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, 0xf6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3a, 0xe6,
68  0x05, 0x69, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0x9f, 0xbe, 0x76,
69  0xc8, 0xb4, 0x39, 0x00, 0xbf, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
70  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00,
71  0x00, 0x00, 0x65, 0x04, 0xbf, 0xc2, 0x6d, 0x90, 0xa0, 0x1d, 0xe7, 0x77, 0xbf, 0xdb, 0xe1, 0x32, 0xf6, 0xa8, 0x66,
72  0x44, 0x3f, 0xc9, 0xdc, 0x1e, 0xb0, 0x03, 0x31, 0xe6, 0xbf, 0x54, 0x02, 0xc0, 0xf8, 0xe6, 0xe6, 0x79, 0x40, 0x08,
73  0xee, 0x22, 0x63, 0x78, 0xfa, 0xe0, 0x3f, 0xa3, 0xe9, 0xa4, 0x23, 0x7a, 0x7b, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00,
74  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
75  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
76  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe1, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
77  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00,
78  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
79  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
80  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33,
81  0x33, 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
82  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc,
83  0xcc, 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
84  0x00, 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f,
85  0xb9, 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00,
86  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45,
87  0x50, 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
88  0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
89  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
90  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
91  0x00, 0x4b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
92  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00,
93  0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x42, 0x40, 0x00, 0x00, 0x00, 0x00,
94  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00,
95  0x25, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
96  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, 0x00, 0x01, 0xbd,
97  0x06, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19,
98  0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28,
99  0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63,
100  0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57,
101  0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c,
102  0x2f, 0x63, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40,
103  0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94,
104  0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6,
105  0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44,
106  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00,
107  0x00, 0x00, 0x00, 0x3f, 0xf0, 0xc1, 0x52, 0x38, 0x2d, 0x73, 0x65, 0x3f, 0xf6, 0x57, 0x18, 0x4a, 0xe7, 0x44, 0x87,
108  0x3f, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf3, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3f, 0xd0, 0x00,
109  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, 0x33,
110  0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
111  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, 0xcc,
112  0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
113  0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, 0xb9,
114  0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00,
115  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50,
116  0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
117  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
118  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
119  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01,
120  0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x3d, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
121  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
122  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
123  0x3f, 0x6c, 0xf5, 0xac, 0x1d, 0xb9, 0xa1, 0x08, 0x00, 0x00, 0x00, 0x09, 0x08, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00,
124  0x00, 0x2b, 0x0a, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0xbf, 0xb8, 0x4d, 0xc2, 0x84, 0x9f, 0xed, 0xcf, 0xbf, 0xb0,
125  0x37, 0x9e, 0xd0, 0x87, 0xba, 0x97, 0x3f, 0xe2, 0xa2, 0x5b, 0x78, 0xc3, 0x9f, 0x6c, 0x3f, 0xc0, 0xa3, 0xd7, 0x0a,
126  0x3d, 0x70, 0xa4, 0x00, 0x00, 0x00, 0x1a, 0x0b, 0x00, 0x00, 0x01, 0xc2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
127  0x00, 0x01, 0x3f, 0xc0, 0x00, 0x00, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0c, 0x00, 0x01, 0x01
128  };
129  comm::BinParser bp(raw_data, sizeof(raw_data));
130 
131  std::vector<std::unique_ptr<primary_interface::PrimaryPackage>> products;
132  primary_interface::PrimaryParser parser;
133  parser.parse(bp, products);
134 
135  EXPECT_EQ(products.size(), 13);
136 
137  if (primary_interface::KinematicsInfo* data = dynamic_cast<primary_interface::KinematicsInfo*>(products[3].get()))
138  {
139  EXPECT_EQ(data->checksum_,
140  vector6uint32_t({ 4294967295, 4294967295, 4294967295, 4294967295, 4294967295, 4294967295 }));
141  EXPECT_EQ(data->dh_theta_, vector6d_t({ 0, 0, 0, 0, 0, 0 }));
142  EXPECT_EQ(data->dh_a_, vector6d_t({ 0, -0.425, -0.3922, 0, 0, 0 }));
143  EXPECT_EQ(data->dh_d_, vector6d_t({ 0.1625, 0, 0, 0.1333, 0.0997, 0.0996 }));
144  EXPECT_EQ(data->dh_alpha_, vector6d_t({ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 }));
145  }
146 }
147 
148 int main(int argc, char* argv[])
149 {
150  ::testing::InitGoogleTest(&argc, argv);
151 
152  return RUN_ALL_TESTS();
153 }
primary_parser.h
urcl::comm::BinParser
The BinParser class handles a byte buffer and functionality to iteratively parse the content...
Definition: bin_parser.h:44
bin_parser.h
urcl::primary_interface::PrimaryParser::parse
bool parse(comm::BinParser &bp, std::vector< std::unique_ptr< PrimaryPackage >> &results)
Uses the given BinParser to create package objects from the contained serialization.
Definition: primary_parser.h:55
urcl::primary_interface::PrimaryParser
The primary specific parser. Interprets a given byte stream as serialized primary packages and parses...
Definition: primary_parser.h:40
urcl::primary_interface::KinematicsInfo
This messages contains information about the robot&#39;s calibration. The DH parameters are a combination...
Definition: kinematics_info.h:42
main
int main(int argc, char *argv[])
Definition: test_primary_parser.cpp:148
urcl::vector6uint32_t
std::array< uint32_t, 6 > vector6uint32_t
Definition: types.h:32
urcl::vector6d_t
std::array< double, 6 > vector6d_t
Definition: types.h:30
TEST
TEST(primary_parser, parse_calibration_data)
Definition: test_primary_parser.cpp:35

ur_client_library
Author(s): Thomas Timm Andersen, Simon Rasmussen, Felix Exner, Lea Steffen, Tristan Schnell
autogenerated on Sun May 9 2021 02:16:26