创建go电机发送反馈串口驱动
This commit is contained in:
parent
078a375ac5
commit
30d3d2fb3a
88
src/hardware/mdog_hardware/CMakeLists.txt
Normal file
88
src/hardware/mdog_hardware/CMakeLists.txt
Normal file
@ -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()
|
||||
@ -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 <string>
|
||||
#include <vector>
|
||||
|
||||
#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<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
|
||||
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
||||
std::vector<hardware_interface::CommandInterface> 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<double> hw_commands_;
|
||||
std::vector<double> hw_states_;
|
||||
};
|
||||
|
||||
} // namespace mdog_hardware
|
||||
|
||||
#endif // MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_
|
||||
@ -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_
|
||||
9
src/hardware/mdog_hardware/mdog_hardware.xml
Normal file
9
src/hardware/mdog_hardware/mdog_hardware.xml
Normal file
@ -0,0 +1,9 @@
|
||||
<library path="mdog_hardware">
|
||||
<class name="mdog_hardware/MDogHardwareInterface"
|
||||
type="mdog_hardware::MDogHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
ros2_control hardware interface.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
30
src/hardware/mdog_hardware/package.xml
Normal file
30
src/hardware/mdog_hardware/package.xml
Normal file
@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>mdog_hardware</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>hardware_interface</depend>
|
||||
|
||||
<depend>pluginlib</depend>
|
||||
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_gmock</test_depend>
|
||||
<test_depend>ros2_control_test_assets</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
110
src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp
Normal file
110
src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp
Normal file
@ -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 <limits>
|
||||
#include <vector>
|
||||
|
||||
#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<double>::quiet_NaN());
|
||||
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::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<hardware_interface::StateInterface> MDogHardwareInterface::export_state_interfaces()
|
||||
{
|
||||
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> MDogHardwareInterface::export_command_interfaces()
|
||||
{
|
||||
std::vector<hardware_interface::CommandInterface> 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)
|
||||
@ -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 <gmock/gmock.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
#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"(
|
||||
<ros2_control name="MDogHardwareInterface2dof" type="system">
|
||||
<hardware>
|
||||
<plugin>mdog_hardware/MDogHardwareInterface</plugin>
|
||||
</hardware>
|
||||
<joint name="joint1">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<param name="initial_position">1.57</param>
|
||||
</joint>
|
||||
<joint name="joint2">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<param name="initial_position">0.7854</param>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
)";
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
86
src/hardware/mdog_hardware_interface/CMakeLists.txt
Normal file
86
src/hardware/mdog_hardware_interface/CMakeLists.txt
Normal file
@ -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()
|
||||
@ -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 <string>
|
||||
#include <vector>
|
||||
|
||||
#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<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
|
||||
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
|
||||
std::vector<hardware_interface::CommandInterface> 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<double> hw_commands_;
|
||||
std::vector<double> hw_states_;
|
||||
};
|
||||
|
||||
} // namespace mdog_hardware_interface
|
||||
|
||||
#endif // MDOG_HARDWARE_INTERFACE__MDOG_HARDWARE_INTERFACE_HPP_
|
||||
@ -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_
|
||||
@ -0,0 +1,9 @@
|
||||
<library path="mdog_hardware_interface">
|
||||
<class name="mdog_hardware_interface/MDogHardwareInterface"
|
||||
type="mdog_hardware_interface::MDogHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
ros2_control hardware interface.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
26
src/hardware/mdog_hardware_interface/package.xml
Normal file
26
src/hardware/mdog_hardware_interface/package.xml
Normal file
@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>mdog_hardware_interface</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="1683502971@qq.com">robofish</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_gmock</test_depend>
|
||||
<test_depend>ros2_control_test_assets</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -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 <limits>
|
||||
#include <vector>
|
||||
|
||||
#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<double>::quiet_NaN());
|
||||
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::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<hardware_interface::StateInterface> MDogHardwareInterface::export_state_interfaces()
|
||||
{
|
||||
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> MDogHardwareInterface::export_command_interfaces()
|
||||
{
|
||||
std::vector<hardware_interface::CommandInterface> 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)
|
||||
@ -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 <gmock/gmock.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
#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"(
|
||||
<ros2_control name="MDogHardwareInterface2dof" type="system">
|
||||
<hardware>
|
||||
<plugin>mdog_hardware_interface/MDogHardwareInterface</plugin>
|
||||
</hardware>
|
||||
<joint name="joint1">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<param name="initial_position">1.57</param>
|
||||
</joint>
|
||||
<joint name="joint2">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<param name="initial_position">0.7854</param>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
)";
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user