Skip to content

Refactor and extend tests #210

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
Open

Refactor and extend tests #210

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
c54b5b5
Improve error messages
christophfroehlich Oct 20, 2025
bbb642c
Split and add new tests, use gmock matcher
christophfroehlich Oct 20, 2025
7e18816
std::source_location::function_name()
christophfroehlich Oct 20, 2025
f83c633
const&
christophfroehlich Oct 20, 2025
aec6d5b
Eigen::Vector
christophfroehlich Oct 20, 2025
c1dc67f
Eigen::Vector
christophfroehlich Oct 20, 2025
effc965
Use c++17 compatible version
christophfroehlich Oct 20, 2025
918e07e
Add Vector6d
christophfroehlich Oct 20, 2025
3bc4fbd
Use correct macro
christophfroehlich Oct 20, 2025
97a45a7
Specify Eigen Vector sizes
christophfroehlich Oct 21, 2025
f5c5fa7
Fix end_effector variable for tests
christophfroehlich Oct 21, 2025
1f9c887
Add tests for wrong frame strings
christophfroehlich Oct 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions kinematics_interface/include/kinematics_interface/kinematics_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,18 @@
#include "rclcpp/logging.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"

// TODO(anyone): Use std::source_location::function_name() once we require C++20
#ifdef _MSC_VER
#define FUNCTION_SIGNATURE __FUNCSIG__
#else
#define FUNCTION_SIGNATURE __PRETTY_FUNCTION__
#endif

namespace kinematics_interface
{

using Vector6d = Eigen::Matrix<double, 6, 1>;

class KinematicsInterface
{
public:
Expand Down
12 changes: 9 additions & 3 deletions kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,15 +79,15 @@ bool KinematicsInterfaceKDL::initialize(
parameters_interface->get_parameter(ns + "base", base_param);
root_name_ = base_param.as_string();
}
else
if (root_name_.empty())
{
root_name_ = robot_tree.getRootSegment()->first;
}

if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
{
RCLCPP_ERROR(
LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(),
LOGGER, "failed to find chain from robot root '%s' to end effector '%s'", root_name_.c_str(),
end_effector_name.c_str());
return false;
}
Expand Down Expand Up @@ -121,6 +121,7 @@ bool KinematicsInterfaceKDL::convert_joint_deltas_to_cartesian_deltas(
!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
!verify_joint_vector(delta_theta))
{
RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
return false;
}

Expand All @@ -144,6 +145,7 @@ bool KinematicsInterfaceKDL::convert_cartesian_deltas_to_joint_deltas(
!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
!verify_joint_vector(delta_theta))
{
RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
return false;
}

Expand All @@ -167,6 +169,7 @@ bool KinematicsInterfaceKDL::calculate_jacobian(
!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
!verify_jacobian(jacobian))
{
RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
return false;
}

Expand All @@ -189,6 +192,7 @@ bool KinematicsInterfaceKDL::calculate_jacobian_inverse(
!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
!verify_jacobian_inverse(jacobian_inverse))
{
RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
return false;
}

Expand All @@ -215,6 +219,7 @@ bool KinematicsInterfaceKDL::calculate_link_transform(
// verify inputs
if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name))
{
RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
return false;
}

Expand All @@ -241,8 +246,9 @@ bool KinematicsInterfaceKDL::calculate_frame_difference(
Eigen::Matrix<double, 6, 1> & delta_x)
{
// verify inputs
if (!verify_initialized() || !verify_period(dt))
if (!verify_period(dt))
{
RCLCPP_ERROR(LOGGER, "Input verification failed in '%s'", FUNCTION_SIGNATURE);
return false;
}

Expand Down
Loading