-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathtest_ChainPublisher.cpp
123 lines (97 loc) · 4.47 KB
/
test_ChainPublisher.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
#include <boost/test/unit_test.hpp>
#include <robot_frames/RobotFrames.hpp>
using namespace robot_frames;
struct Fixture {
Fixture() {
// set up fixture
joints.time = base::Time::fromMicroseconds(1234);
joints.names.push_back("kuka_lbr_l_joint_1");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_l_joint_2");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_l_joint_3");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_l_joint_4");
joints.elements.push_back(base::JointState::Position(-0.00));
joints.names.push_back("kuka_lbr_l_joint_5");
joints.elements.push_back(base::JointState::Position(-0.0));
joints.names.push_back("kuka_lbr_l_joint_6");
joints.elements.push_back(base::JointState::Position(-0.0));
joints.names.push_back("kuka_lbr_l_joint_7");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_r_joint_1");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_r_joint_2");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_r_joint_3");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_r_joint_4");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_r_joint_5");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_r_joint_6");
joints.elements.push_back(base::JointState::Position(0.0));
joints.names.push_back("kuka_lbr_r_joint_7");
joints.elements.push_back(base::JointState::Position(0.0));
// base -> kuka_lbr_cam_calib_marker
robot_frames::Chain chain1;
chain1.name = "kuka_lbr_cam_calib_marker";
chain1.root_link = "kuka_lbr_center";
chain1.tip_link = "kuka_lbr_r_link_2";
// base -> kuka_lbr_front_left_camera
robot_frames::Chain chain2;
chain2.name = "kuka_lbr_front_left_camera";
chain2.root_link = "__base__";
chain2.tip_link = "kuka_lbr_front_left_camera";
// kuka_lbr_center -> kuka_lbr_r_link_1
robot_frames::Chain chain3;
chain3.name = "kuka_lbr_r_link_1";
chain3.root_link = "kuka_lbr_center";
chain3.tip_link = "kuka_lbr_r_link_1";
chains_1.push_back(chain1);
chains_2.push_back(chain1);
chains_2.push_back(chain2);
chains_3.push_back(chain1);
chains_3.push_back(chain2);
chains_3.push_back(chain3);
// urdf path
std::ostringstream ss;
ss << std::getenv("ROCK_PREFIX") << "/../robot_frames/test/kuka_lbr_no_meshes.urdf";
urdf_path = ss.str();
}
~Fixture() {
// tear down fixture
}
ChainTransformationCalculator* transformer;
std::string urdf_path;
// create chains
std::vector<robot_frames::Chain> chains_1;
std::vector<robot_frames::Chain> chains_2;
std::vector<robot_frames::Chain> chains_3;
// create joint state
base::samples::Joints joints;
};
BOOST_FIXTURE_TEST_SUITE( Test_fixture, Fixture )
BOOST_AUTO_TEST_CASE(should_work_with_1_chain)
{
std::vector<base::samples::RigidBodyState> btframes;
ChainTransformationCalculator *transformer = new ChainTransformationCalculator(chains_1, urdf_path);
transformer->update_transforms(joints, btframes);
}
BOOST_AUTO_TEST_CASE(should_work_with_2_chain)
{
std::vector<base::samples::RigidBodyState> btframes;
ChainTransformationCalculator *transformer = new ChainTransformationCalculator(chains_2, urdf_path);
transformer->update_transforms(joints, btframes);
}
BOOST_AUTO_TEST_CASE(should_work_with_3_chain)
{
std::vector<base::samples::RigidBodyState> btframes;
ChainTransformationCalculator *transformer = new ChainTransformationCalculator(chains_3, urdf_path);
transformer->update_transforms(joints, btframes);
// for(const auto btframe: btframes){
// std::cout << btframe.sourceFrame << "-->" << btframe.targetFrame << std::endl;
// std::cout << btframe.position[0] << " >=> " << btframe.position[1] << " >=> "<< btframe.position[2] << " >=> " << std::endl;
// }
}
BOOST_AUTO_TEST_SUITE_END()