diff --git a/src/hardware/mdog_hardware/CMakeLists.txt b/src/hardware/mdog_hardware/CMakeLists.txt new file mode 100644 index 0000000..d5d1337 --- /dev/null +++ b/src/hardware/mdog_hardware/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.8) +project(mdog_hardware) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +add_library( + mdog_hardware + SHARED + src/mdog_hardware_interface.cpp +) +target_include_directories( + mdog_hardware + PUBLIC + include +) +ament_target_dependencies( + mdog_hardware + hardware_interface + rclcpp + rclcpp_lifecycle +) +# prevent pluginlib from using boost +target_compile_definitions(mdog_hardware PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file( + hardware_interface mdog_hardware.xml) + +install( + TARGETS + mdog_hardware + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_mdog_hardware_interface test/test_mdog_hardware_interface.cpp) + target_include_directories(test_mdog_hardware_interface PRIVATE include) + ament_target_dependencies( + test_mdog_hardware_interface + hardware_interface + pluginlib + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + mdog_hardware +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +ament_package() diff --git a/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp b/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp new file mode 100644 index 0000000..b2c57fd --- /dev/null +++ b/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp @@ -0,0 +1,71 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#ifndef MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_ +#define MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_ + +#include +#include + +#include "mdog_hardware/visibility_control.h" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace mdog_hardware +{ +class MDogHardwareInterface : public hardware_interface::SystemInterface +{ +public: + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::vector hw_commands_; + std::vector hw_states_; +}; + +} // namespace mdog_hardware + +#endif // MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h b/src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h new file mode 100644 index 0000000..255b7fb --- /dev/null +++ b/src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#ifndef MDOG_HARDWARE__VISIBILITY_CONTROL_H_ +#define MDOG_HARDWARE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // MDOG_HARDWARE__VISIBILITY_CONTROL_H_ diff --git a/src/hardware/mdog_hardware/mdog_hardware.xml b/src/hardware/mdog_hardware/mdog_hardware.xml new file mode 100644 index 0000000..78fc04d --- /dev/null +++ b/src/hardware/mdog_hardware/mdog_hardware.xml @@ -0,0 +1,9 @@ + + + + ros2_control hardware interface. + + + diff --git a/src/hardware/mdog_hardware/package.xml b/src/hardware/mdog_hardware/package.xml new file mode 100644 index 0000000..7e4d806 --- /dev/null +++ b/src/hardware/mdog_hardware/package.xml @@ -0,0 +1,30 @@ + + + + mdog_hardware + 0.0.0 + TODO: Package description + robofish + TODO: License declaration + + ament_cmake + + hardware_interface + + pluginlib + + rclcpp_lifecycle + + rclcpp + std_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gmock + ros2_control_test_assets + + + ament_cmake + + diff --git a/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp b/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp new file mode 100644 index 0000000..2929155 --- /dev/null +++ b/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp @@ -0,0 +1,110 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#include +#include + +#include "mdog_hardware/mdog_hardware_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace mdog_hardware +{ +hardware_interface::CallbackReturn MDogHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + // TODO(anyone): read parameters and initialize the hardware + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn MDogHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces + + return CallbackReturn::SUCCESS; +} + +std::vector MDogHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + for (size_t i = 0; i < info_.joints.size(); ++i) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + // TODO(anyone): insert correct interfaces + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + } + + return state_interfaces; +} + +std::vector MDogHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + for (size_t i = 0; i < info_.joints.size(); ++i) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + // TODO(anyone): insert correct interfaces + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn MDogHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): prepare the robot to receive commands + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn MDogHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): prepare the robot to stop receiving commands + + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type MDogHardwareInterface::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // TODO(anyone): read robot states + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MDogHardwareInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // TODO(anyone): write robot's commands' + + return hardware_interface::return_type::OK; +} + +} // namespace mdog_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + mdog_hardware::MDogHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp b/src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp new file mode 100644 index 0000000..2262ac4 --- /dev/null +++ b/src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp @@ -0,0 +1,57 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#include + +#include + +#include "hardware_interface/resource_manager.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +class TestMDogHardwareInterface : public ::testing::Test +{ +protected: + void SetUp() override + { + // TODO(anyone): Extend this description to your robot + mdog_hardware_interface_2dof_ = + R"( + + + mdog_hardware/MDogHardwareInterface + + + + + 1.57 + + + + + 0.7854 + + + )"; + } + + std::string mdog_hardware_interface_2dof_; +}; + +TEST_F(TestMDogHardwareInterface, load_mdog_hardware_interface_2dof) +{ + auto urdf = ros2_control_test_assets::urdf_head + mdog_hardware_interface_2dof_ + + ros2_control_test_assets::urdf_tail; + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); +} diff --git a/src/hardware/mdog_hardware_interface/CMakeLists.txt b/src/hardware/mdog_hardware_interface/CMakeLists.txt new file mode 100644 index 0000000..87ba48d --- /dev/null +++ b/src/hardware/mdog_hardware_interface/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.8) +project(mdog_hardware_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) + +add_library( + mdog_hardware_interface + SHARED + src/mdog_hardware_interface.cpp +) +target_include_directories( + mdog_hardware_interface + PUBLIC + include +) +ament_target_dependencies( + mdog_hardware_interface + hardware_interface + rclcpp + rclcpp_lifecycle +) +# prevent pluginlib from using boost +target_compile_definitions(mdog_hardware_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file( + hardware_interface mdog_hardware_interface.xml) + +install( + TARGETS + mdog_hardware_interface + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_mdog_hardware_interface test/test_mdog_hardware_interface.cpp) + target_include_directories(test_mdog_hardware_interface PRIVATE include) + ament_target_dependencies( + test_mdog_hardware_interface + hardware_interface + pluginlib + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + mdog_hardware_interface +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +ament_package() diff --git a/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/mdog_hardware_interface.hpp b/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/mdog_hardware_interface.hpp new file mode 100644 index 0000000..58b9061 --- /dev/null +++ b/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/mdog_hardware_interface.hpp @@ -0,0 +1,71 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#ifndef MDOG_HARDWARE_INTERFACE__MDOG_HARDWARE_INTERFACE_HPP_ +#define MDOG_HARDWARE_INTERFACE__MDOG_HARDWARE_INTERFACE_HPP_ + +#include +#include + +#include "mdog_hardware_interface/visibility_control.h" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace mdog_hardware_interface +{ +class MDogHardwareInterface : public hardware_interface::SystemInterface +{ +public: + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::vector hw_commands_; + std::vector hw_states_; +}; + +} // namespace mdog_hardware_interface + +#endif // MDOG_HARDWARE_INTERFACE__MDOG_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/visibility_control.h b/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/visibility_control.h new file mode 100644 index 0000000..15be442 --- /dev/null +++ b/src/hardware/mdog_hardware_interface/include/mdog_hardware_interface/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#ifndef MDOG_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ +#define MDOG_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // MDOG_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ diff --git a/src/hardware/mdog_hardware_interface/mdog_hardware_interface.xml b/src/hardware/mdog_hardware_interface/mdog_hardware_interface.xml new file mode 100644 index 0000000..48f25f8 --- /dev/null +++ b/src/hardware/mdog_hardware_interface/mdog_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + ros2_control hardware interface. + + + diff --git a/src/hardware/mdog_hardware_interface/package.xml b/src/hardware/mdog_hardware_interface/package.xml new file mode 100644 index 0000000..31a398f --- /dev/null +++ b/src/hardware/mdog_hardware_interface/package.xml @@ -0,0 +1,26 @@ + + + + mdog_hardware_interface + 0.0.0 + TODO: Package description + robofish + TODO: License declaration + + ament_cmake + + rclcpp_lifecycle + + rclcpp + hardware_interface + pluginlib + + ament_lint_auto + ament_lint_common + ament_cmake_gmock + ros2_control_test_assets + + + ament_cmake + + diff --git a/src/hardware/mdog_hardware_interface/src/mdog_hardware_interface.cpp b/src/hardware/mdog_hardware_interface/src/mdog_hardware_interface.cpp new file mode 100644 index 0000000..89b2b0b --- /dev/null +++ b/src/hardware/mdog_hardware_interface/src/mdog_hardware_interface.cpp @@ -0,0 +1,110 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#include +#include + +#include "mdog_hardware_interface/mdog_hardware_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace mdog_hardware_interface +{ +hardware_interface::CallbackReturn MDogHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + // TODO(anyone): read parameters and initialize the hardware + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn MDogHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces + + return CallbackReturn::SUCCESS; +} + +std::vector MDogHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + for (size_t i = 0; i < info_.joints.size(); ++i) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + // TODO(anyone): insert correct interfaces + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + } + + return state_interfaces; +} + +std::vector MDogHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + for (size_t i = 0; i < info_.joints.size(); ++i) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + // TODO(anyone): insert correct interfaces + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn MDogHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): prepare the robot to receive commands + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn MDogHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): prepare the robot to stop receiving commands + + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type MDogHardwareInterface::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // TODO(anyone): read robot states + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type MDogHardwareInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // TODO(anyone): write robot's commands' + + return hardware_interface::return_type::OK; +} + +} // namespace mdog_hardware_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + mdog_hardware_interface::MDogHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware/mdog_hardware_interface/test/test_mdog_hardware_interface.cpp b/src/hardware/mdog_hardware_interface/test/test_mdog_hardware_interface.cpp new file mode 100644 index 0000000..7144ae6 --- /dev/null +++ b/src/hardware/mdog_hardware_interface/test/test_mdog_hardware_interface.cpp @@ -0,0 +1,57 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#include + +#include + +#include "hardware_interface/resource_manager.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +class TestMDogHardwareInterface : public ::testing::Test +{ +protected: + void SetUp() override + { + // TODO(anyone): Extend this description to your robot + mdog_hardware_interface_2dof_ = + R"( + + + mdog_hardware_interface/MDogHardwareInterface + + + + + 1.57 + + + + + 0.7854 + + + )"; + } + + std::string mdog_hardware_interface_2dof_; +}; + +TEST_F(TestMDogHardwareInterface, load_mdog_hardware_interface_2dof) +{ + auto urdf = ros2_control_test_assets::urdf_head + mdog_hardware_interface_2dof_ + + ros2_control_test_assets::urdf_tail; + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); +}