Skip to content

Commit 4ab5ea5

Browse files
christophfroehlichchristophfroehlich
authored
Merge branch 'master' into rm/iron
2 parents 4d3d548 + 4d6bada commit 4ab5ea5

File tree

8 files changed

+94
-24
lines changed

8 files changed

+94
-24
lines changed

‎kinematics_interface/CHANGELOG.rst

+8
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,14 @@
22
Changelog for package kinematics_interface
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
1.2.1 (2024-11-11)
6+
------------------
7+
8+
1.2.0 (2024-11-05)
9+
------------------
10+
* API changes to support robot description (`#83 <https://github.com/ros-controls/kinematics_interface/issues/83>`_)
11+
* Contributors: Dr. Denis
12+
513
1.1.0 (2024-05-20)
614
------------------
715
* Use CMake targets for eigen (`#50 <https://github.com/ros-controls/kinematics_interface/issues/50>`_)

‎kinematics_interface/include/kinematics_interface/kinematics_interface.hpp

+6-1
Original file line numberDiff line numberDiff line change
@@ -39,10 +39,15 @@ class KinematicsInterface
3939

4040
/**
4141
* \brief Initialize plugin. This method must be called before any other.
42+
* \param[in] robot_description robot URDF in string format
43+
* \param[in] parameters_interface
44+
* \param[in] param_namespace namespace for kinematics parameters - defaults to "kinematics"
45+
* \return true if successful
4246
*/
4347
virtual bool initialize(
48+
const std::string & robot_description,
4449
std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
45-
const std::string & end_effector_name) = 0;
50+
const std::string & param_namespace) = 0;
4651

4752
/**
4853
* \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian.

‎kinematics_interface/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>kinematics_interface</name>
5-
<version>1.1.0</version>
5+
<version>1.2.1</version>
66
<description>Kinematics interface for ROS 2 control</description>
77
<maintainer email="[email protected]">Denis Štogl</maintainer>
88
<maintainer email="[email protected]">Bence Magyar</maintainer>

‎kinematics_interface_kdl/CHANGELOG.rst

+10
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,16 @@
22
Changelog for package kinematics_interface_kdl
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
1.2.1 (2024-11-11)
6+
------------------
7+
* Remove ns from robot_description parameter (`#91 <https://github.com/ros-controls/kinematics_interface/issues/91>`_)
8+
* Contributors: Christoph Fröhlich
9+
10+
1.2.0 (2024-11-05)
11+
------------------
12+
* API changes to support robot description (`#83 <https://github.com/ros-controls/kinematics_interface/issues/83>`_)
13+
* Contributors: Dr. Denis
14+
515
1.1.0 (2024-05-20)
616
------------------
717
* Read base parameter in initialize function (`#73 <https://github.com/ros-controls/kinematics_interface/issues/73>`_)

‎kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -40,8 +40,9 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface
4040
{
4141
public:
4242
bool initialize(
43+
const std::string & robot_description,
4344
std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
44-
const std::string & end_effector_name) override;
45+
const std::string & param_namespace) override;
4546

4647
bool convert_cartesian_deltas_to_joint_deltas(
4748
const Eigen::VectorXd & joint_pos, const Eigen::Matrix<double, 6, 1> & delta_x,

‎kinematics_interface_kdl/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>kinematics_interface_kdl</name>
5-
<version>1.1.0</version>
5+
<version>1.2.1</version>
66
<description>KDL implementation of ros2_control kinematics interface</description>
77
<maintainer email="[email protected]">Bence Magyar</maintainer>
88
<maintainer email="[email protected]">Denis Štogl</maintainer>

‎kinematics_interface_kdl/src/kinematics_interface_kdl.cpp

+39-13
Original file line numberDiff line numberDiff line change
@@ -21,36 +21,62 @@ namespace kinematics_interface_kdl
2121
rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_kdl");
2222

2323
bool KinematicsInterfaceKDL::initialize(
24+
const std::string & robot_description,
2425
std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
25-
const std::string & end_effector_name)
26+
const std::string & param_namespace)
2627
{
2728
// track initialization plugin
2829
initialized = true;
2930

30-
// get robot description
31-
auto robot_param = rclcpp::Parameter();
32-
if (!parameters_interface->get_parameter("robot_description", robot_param))
31+
// get parameters
32+
std::string ns = !param_namespace.empty() ? param_namespace + "." : "";
33+
34+
std::string robot_description_local;
35+
if (robot_description.empty())
3336
{
34-
RCLCPP_ERROR(LOGGER, "parameter robot_description not set");
35-
return false;
37+
// If the robot_description input argument is empty, try to get the
38+
// robot_description from the node's parameters.
39+
auto robot_param = rclcpp::Parameter();
40+
if (!parameters_interface->get_parameter("robot_description", robot_param))
41+
{
42+
RCLCPP_ERROR(LOGGER, "parameter robot_description not set in kinematics_interface_kdl");
43+
return false;
44+
}
45+
robot_description_local = robot_param.as_string();
3646
}
37-
auto robot_description = robot_param.as_string();
47+
else
48+
{
49+
robot_description_local = robot_description;
50+
}
51+
3852
// get alpha damping term
3953
auto alpha_param = rclcpp::Parameter("alpha", 0.000005);
40-
if (parameters_interface->has_parameter("alpha"))
54+
if (parameters_interface->has_parameter(ns + "alpha"))
4155
{
42-
parameters_interface->get_parameter("alpha", alpha_param);
56+
parameters_interface->get_parameter(ns + "alpha", alpha_param);
4357
}
4458
alpha = alpha_param.as_double();
59+
// get end-effector name
60+
auto end_effector_name_param = rclcpp::Parameter("tip");
61+
if (parameters_interface->has_parameter(ns + "tip"))
62+
{
63+
parameters_interface->get_parameter(ns + "tip", end_effector_name_param);
64+
}
65+
else
66+
{
67+
RCLCPP_ERROR(LOGGER, "Failed to find end effector name parameter [tip].");
68+
return false;
69+
}
70+
std::string end_effector_name = end_effector_name_param.as_string();
71+
4572
// create kinematic chain
4673
KDL::Tree robot_tree;
47-
kdl_parser::treeFromString(robot_description, robot_tree);
48-
74+
kdl_parser::treeFromString(robot_description_local, robot_tree);
4975
// get root name
5076
auto base_param = rclcpp::Parameter();
51-
if (parameters_interface->has_parameter("base"))
77+
if (parameters_interface->has_parameter(ns + "base"))
5278
{
53-
parameters_interface->get_parameter("base", base_param);
79+
parameters_interface->get_parameter(ns + "base", base_param);
5480
root_name_ = base_param.as_string();
5581
}
5682
else

‎kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp

+27-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,28 @@ 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+
rclcpp::Parameter param("tip", "link2");
74+
node_->declare_parameter("tip", "link2");
75+
node_->set_parameter(param);
76+
}
6677
};
6778

6879
TEST_F(TestKDLPlugin, KDL_plugin_function)
6980
{
7081
// load robot description and alpha to parameter server
7182
loadURDFParameter();
7283
loadAlphaParameter();
84+
loadTipParameter();
7385

7486
// initialize the plugin
75-
ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_));
87+
ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), ""));
7688

7789
// calculate end effector transform
7890
Eigen::Matrix<double, Eigen::Dynamic, 1> pos = Eigen::Matrix<double, 2, 1>::Zero();
@@ -103,9 +115,10 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
103115
// load robot description and alpha to parameter server
104116
loadURDFParameter();
105117
loadAlphaParameter();
118+
loadTipParameter();
106119

107120
// initialize the plugin
108-
ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_));
121+
ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), ""));
109122

110123
// calculate end effector transform
111124
std::vector<double> pos = {0, 0};
@@ -136,9 +149,10 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
136149
// load robot description and alpha to parameter server
137150
loadURDFParameter();
138151
loadAlphaParameter();
152+
loadTipParameter();
139153

140154
// initialize the plugin
141-
ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_));
155+
ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), ""));
142156

143157
// define correct values
144158
Eigen::Matrix<double, Eigen::Dynamic, 1> pos = Eigen::Matrix<double, 2, 1>::Zero();
@@ -175,5 +189,11 @@ TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)
175189
{
176190
// load alpha to parameter server
177191
loadAlphaParameter();
178-
ASSERT_FALSE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_));
192+
loadTipParameter();
193+
ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), ""));
194+
}
195+
196+
TEST_F(TestKDLPlugin, KDL_plugin_no_parameter_tip)
197+
{
198+
ASSERT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), ""));
179199
}

0 commit comments

Comments
 (0)