From eacb9f498e8007d495410707ba83947d79209950 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 19 Feb 2024 21:24:30 +0000 Subject: [PATCH] Move definition logger to cpp to avoid "multiple definition" linker error (backport #21) (#32) --- kinematics_interface/CMakeLists.txt | 10 +-- .../kinematics_interface.hpp | 52 +----------- .../src/kinematics_interface.cpp | 83 +++++++++++++++++++ 3 files changed, 92 insertions(+), 53 deletions(-) create mode 100644 kinematics_interface/src/kinematics_interface.cpp diff --git a/kinematics_interface/CMakeLists.txt b/kinematics_interface/CMakeLists.txt index 6bfc63d..7decbdb 100644 --- a/kinematics_interface/CMakeLists.txt +++ b/kinematics_interface/CMakeLists.txt @@ -18,19 +18,19 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() # Create interface library for kinematics base class -add_library(kinematics_interface INTERFACE) -target_compile_features(kinematics_interface INTERFACE cxx_std_17) -target_include_directories(kinematics_interface INTERFACE +add_library(kinematics_interface SHARED src/kinematics_interface.cpp) +target_compile_features(kinematics_interface PUBLIC cxx_std_17) +target_include_directories(kinematics_interface PUBLIC $ $ ) -ament_target_dependencies(kinematics_interface INTERFACE +ament_target_dependencies(kinematics_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(kinematics_interface INTERFACE "KINEMATICS_INTERFACE_BUILDING_DLL") +target_compile_definitions(kinematics_interface PUBLIC "KINEMATICS_INTERFACE_BUILDING_DLL") install( DIRECTORY include/ diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index 80763af..b3d4cee 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -30,8 +30,6 @@ namespace kinematics_interface { -rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface"); - class KinematicsInterface { public: @@ -94,61 +92,19 @@ class KinematicsInterface bool convert_cartesian_deltas_to_joint_deltas( std::vector & joint_pos_vec, const std::vector & delta_x_vec, - const std::string & link_name, std::vector & delta_theta_vec) - { - auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); - auto delta_x = Eigen::Map(delta_x_vec.data(), delta_x_vec.size()); - // TODO(anyone): heap allocation should be removed for realtime use - Eigen::VectorXd delta_theta = - Eigen::Map(delta_theta_vec.data(), delta_theta_vec.size()); - - bool ret = convert_cartesian_deltas_to_joint_deltas(joint_pos, delta_x, link_name, delta_theta); - for (auto i = 0ul; i < delta_theta_vec.size(); i++) - { - delta_theta_vec[i] = delta_theta[i]; - } - return ret; - } + const std::string & link_name, std::vector & delta_theta_vec); bool convert_joint_deltas_to_cartesian_deltas( const std::vector & joint_pos_vec, const std::vector & delta_theta_vec, - const std::string & link_name, std::vector & delta_x_vec) - { - auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); - Eigen::VectorXd delta_theta = - Eigen::Map(delta_theta_vec.data(), delta_theta_vec.size()); - if (delta_x_vec.size() != 6) - { - RCLCPP_ERROR( - LOGGER, "The length of the cartesian delta vector (%zu) must be 6.", delta_x_vec.size()); - return false; - } - Eigen::Matrix delta_x(delta_x_vec.data()); - bool ret = convert_joint_deltas_to_cartesian_deltas(joint_pos, delta_theta, link_name, delta_x); - for (auto i = 0ul; i < delta_x_vec.size(); i++) - { - delta_x_vec[i] = delta_x[i]; - } - return ret; - } + const std::string & link_name, std::vector & delta_x_vec); bool calculate_link_transform( const std::vector & joint_pos_vec, const std::string & link_name, - Eigen::Isometry3d & transform) - { - auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); - - return calculate_link_transform(joint_pos, link_name, transform); - } + Eigen::Isometry3d & transform); bool calculate_jacobian( const std::vector & joint_pos_vec, const std::string & link_name, - Eigen::Matrix & jacobian) - { - auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); - - return calculate_jacobian(joint_pos, link_name, jacobian); - } + Eigen::Matrix & jacobian); }; } // namespace kinematics_interface diff --git a/kinematics_interface/src/kinematics_interface.cpp b/kinematics_interface/src/kinematics_interface.cpp new file mode 100644 index 0000000..a3b94b5 --- /dev/null +++ b/kinematics_interface/src/kinematics_interface.cpp @@ -0,0 +1,83 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Andy Zelenak, Paul Gesel +/// \description: Base class for kinematics interface + +#include "kinematics_interface/kinematics_interface.hpp" + +namespace kinematics_interface +{ + +rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface"); + +bool KinematicsInterface::convert_cartesian_deltas_to_joint_deltas( + std::vector & joint_pos_vec, const std::vector & delta_x_vec, + const std::string & link_name, std::vector & delta_theta_vec) +{ + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + auto delta_x = Eigen::Map(delta_x_vec.data(), delta_x_vec.size()); + // TODO(anyone): heap allocation should be removed for realtime use + Eigen::VectorXd delta_theta = + Eigen::Map(delta_theta_vec.data(), delta_theta_vec.size()); + + bool ret = convert_cartesian_deltas_to_joint_deltas(joint_pos, delta_x, link_name, delta_theta); + for (auto i = 0ul; i < delta_theta_vec.size(); i++) + { + delta_theta_vec[i] = delta_theta[i]; + } + return ret; +} + +bool KinematicsInterface::convert_joint_deltas_to_cartesian_deltas( + const std::vector & joint_pos_vec, const std::vector & delta_theta_vec, + const std::string & link_name, std::vector & delta_x_vec) +{ + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + Eigen::VectorXd delta_theta = + Eigen::Map(delta_theta_vec.data(), delta_theta_vec.size()); + if (delta_x_vec.size() != 6) + { + RCLCPP_ERROR( + LOGGER, "The length of the cartesian delta vector (%zu) must be 6.", delta_x_vec.size()); + return false; + } + Eigen::Matrix delta_x(delta_x_vec.data()); + bool ret = convert_joint_deltas_to_cartesian_deltas(joint_pos, delta_theta, link_name, delta_x); + for (auto i = 0ul; i < delta_x_vec.size(); i++) + { + delta_x_vec[i] = delta_x[i]; + } + return ret; +} + +bool KinematicsInterface::calculate_link_transform( + const std::vector & joint_pos_vec, const std::string & link_name, + Eigen::Isometry3d & transform) +{ + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + + return calculate_link_transform(joint_pos, link_name, transform); +} + +bool KinematicsInterface::calculate_jacobian( + const std::vector & joint_pos_vec, const std::string & link_name, + Eigen::Matrix & jacobian) +{ + auto joint_pos = Eigen::Map(joint_pos_vec.data(), joint_pos_vec.size()); + + return calculate_jacobian(joint_pos, link_name, jacobian); +} + +} // namespace kinematics_interface