Skip to content

Commit daf0f55

Browse files
committed
adjust API in tests as well
1 parent 24ba1d6 commit daf0f55

File tree

1 file changed

+23
-7
lines changed

1 file changed

+23
-7
lines changed

kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp

+23-7
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,8 @@ class TestKDLPlugin : public ::testing::Test
2828
std::shared_ptr<kinematics_interface::KinematicsInterface> ik_;
2929
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
3030
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);
3133

3234
void SetUp()
3335
{
@@ -50,9 +52,7 @@ class TestKDLPlugin : public ::testing::Test
5052

5153
void loadURDFParameter()
5254
{
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_);
5656
node_->declare_parameter("robot_description", "");
5757
node_->set_parameter(param);
5858
}
@@ -63,16 +63,29 @@ class TestKDLPlugin : public ::testing::Test
6363
node_->declare_parameter("alpha", 0.005);
6464
node_->set_parameter(param);
6565
}
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+
}
6678
};
6779

6880
TEST_F(TestKDLPlugin, KDL_plugin_function)
6981
{
7082
// load robot description and alpha to parameter server
7183
loadURDFParameter();
7284
loadAlphaParameter();
85+
loadTipParameter();
7386

7487
// 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(), ""));
7689

7790
// calculate end effector transform
7891
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)
103116
// load robot description and alpha to parameter server
104117
loadURDFParameter();
105118
loadAlphaParameter();
119+
loadTipParameter();
106120

107121
// 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(), ""));
109123

110124
// calculate end effector transform
111125
std::vector<double> pos = {0, 0};
@@ -136,9 +150,10 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
136150
// load robot description and alpha to parameter server
137151
loadURDFParameter();
138152
loadAlphaParameter();
153+
loadTipParameter();
139154

140155
// 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(), ""));
142157

143158
// define correct values
144159
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)
175190
{
176191
// load alpha to parameter server
177192
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(), ""));
179195
}

0 commit comments

Comments
 (0)