Fix tests

[bmagyar]

A clean build of the repo produces a test failure:

$ colcon test-result --verbose
build/kinematics_interface_kdl/Testing/20220804-2210/Test.xml: 1 test, 0 errors, 1 failure, 0 skipped
- test_kinematics_interface_kdl
  <<< failure message
    -- run_test.py: invoking following command in '/tmp/ssss_ws/build/kinematics_interface_kdl':
     - /tmp/ssss_ws/build/kinematics_interface_kdl/test_kinematics_interface_kdl --gtest_output=xml:/tmp/ssss_ws/build/kinematics_interface_kdl/test_results/kinematics_interface_kdl/test_kinematics_interface_kdl.gtest.xml
    Running main() from gmock_main.cc
    [==========] Running 2 tests from 1 test suite.
    [----------] Global test environment set-up.
    [----------] 2 tests from TestKDLPlugin
    [ RUN      ] TestKDLPlugin.KDL_plugin_function
    Link base_link had 1 children
    Link link1 had 1 children
    Link link2 had 1 children
    Link tool_link had 0 children
    free(): invalid next size (fast)
    -- run_test.py: return code -6
    -- run_test.py: generate result file '/tmp/ssss_ws/build/kinematics_interface_kdl/test_results/kinematics_interface_kdl/test_kinematics_interface_kdl.gtest.xml' with failed test
    -- run_test.py: verify result file '/tmp/ssss_ws/build/kinematics_interface_kdl/test_results/kinematics_interface_kdl/test_kinematics_interface_kdl.gtest.xml'
  >>>
build/kinematics_interface_kdl/test_results/kinematics_interface_kdl/test_kinematics_interface_kdl.gtest.xml: 1 test, 1 error, 0 failures, 0 skipped
- kinematics_interface_kdl test_kinematics_interface_kdl.gtest.missing_result
  <<< error message
    The test did not generate a result file:
    
    Running main() from gmock_main.cc
    [==========] Running 2 tests from 1 test suite.
    [----------] Global test environment set-up.
    [----------] 2 tests from TestKDLPlugin
    [ RUN      ] TestKDLPlugin.KDL_plugin_function
    Link base_link had 1 children
    Link link1 had 1 children
    Link link2 had 1 children
    Link tool_link had 0 children
    free(): invalid next size (fast)
  >>>

Summary: 2 tests, 1 error, 1 failure, 0 skipped

[bmagyar]

@pac48 could you please fix this up?

[christophfroehlich]

fixed with #5