@@ -28,6 +28,8 @@ class TestKDLPlugin : public ::testing::Test
28
28
std::shared_ptr<kinematics_interface::KinematicsInterface> ik_;
29
29
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
30
30
std::string end_effector_ = " link2" ;
31
+ std::string urdf_ = std::string(ros2_control_test_assets::urdf_head) +
32
+ std::string (ros2_control_test_assets::urdf_tail);
31
33
32
34
void SetUp ()
33
35
{
@@ -50,9 +52,7 @@ class TestKDLPlugin : public ::testing::Test
50
52
51
53
void loadURDFParameter ()
52
54
{
53
- auto urdf = std::string (ros2_control_test_assets::urdf_head) +
54
- std::string (ros2_control_test_assets::urdf_tail);
55
- rclcpp::Parameter param (" robot_description" , urdf);
55
+ rclcpp::Parameter param (" robot_description" , urdf_);
56
56
node_->declare_parameter (" robot_description" , " " );
57
57
node_->set_parameter (param);
58
58
}
@@ -63,16 +63,29 @@ class TestKDLPlugin : public ::testing::Test
63
63
node_->declare_parameter (" alpha" , 0.005 );
64
64
node_->set_parameter (param);
65
65
}
66
+
67
+ /* *
68
+ * \brief Used for testing initialization from parameters.
69
+ * Elsewhere, `end_effector_` member is used.
70
+ */
71
+ void loadTipParameter ()
72
+ {
73
+
74
+ rclcpp::Parameter param (" tip" , " link2" );
75
+ node_->declare_parameter (" tip" , " link2" );
76
+ node_->set_parameter (param);
77
+ }
66
78
};
67
79
68
80
TEST_F (TestKDLPlugin, KDL_plugin_function)
69
81
{
70
82
// load robot description and alpha to parameter server
71
83
loadURDFParameter ();
72
84
loadAlphaParameter ();
85
+ loadTipParameter ();
73
86
74
87
// initialize the plugin
75
- ASSERT_TRUE (ik_->initialize (node_->get_node_parameters_interface (), end_effector_ ));
88
+ ASSERT_TRUE (ik_->initialize (urdf_, node_->get_node_parameters_interface (), " " ));
76
89
77
90
// calculate end effector transform
78
91
Eigen::Matrix<double , Eigen::Dynamic, 1 > pos = Eigen::Matrix<double , 2 , 1 >::Zero ();
@@ -103,9 +116,10 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
103
116
// load robot description and alpha to parameter server
104
117
loadURDFParameter ();
105
118
loadAlphaParameter ();
119
+ loadTipParameter ();
106
120
107
121
// initialize the plugin
108
- ASSERT_TRUE (ik_->initialize (node_->get_node_parameters_interface (), end_effector_ ));
122
+ ASSERT_TRUE (ik_->initialize (urdf_, node_->get_node_parameters_interface (), " " ));
109
123
110
124
// calculate end effector transform
111
125
std::vector<double > pos = {0 , 0 };
@@ -136,9 +150,10 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
136
150
// load robot description and alpha to parameter server
137
151
loadURDFParameter ();
138
152
loadAlphaParameter ();
153
+ loadTipParameter ();
139
154
140
155
// initialize the plugin
141
- ASSERT_TRUE (ik_->initialize (node_->get_node_parameters_interface (), end_effector_ ));
156
+ ASSERT_TRUE (ik_->initialize (urdf_, node_->get_node_parameters_interface (), " " ));
142
157
143
158
// define correct values
144
159
Eigen::Matrix<double , Eigen::Dynamic, 1 > pos = Eigen::Matrix<double , 2 , 1 >::Zero ();
@@ -175,5 +190,6 @@ TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)
175
190
{
176
191
// load alpha to parameter server
177
192
loadAlphaParameter ();
178
- ASSERT_FALSE (ik_->initialize (node_->get_node_parameters_interface (), end_effector_));
193
+ loadTipParameter ();
194
+ ASSERT_FALSE (ik_->initialize (" " , node_->get_node_parameters_interface (), " " ));
179
195
}
0 commit comments