编写和机器人的硬件通信组件
- 1. 编写Hardware Component代码
- 1.1 创建包
- 1.2 创建源文件
- 1.3 向头文件(.hpp)中添加声明
- 1.4 在源文件(.cpp)中添加定义
- 1.1.1 Include 硬件接口的头文件,并添加 namespace 以简化开发。
- 1.1.2 实现on_init方法。
- 1.1.3 实现on_configure方法
- 1.1.4 实现`on_cleap`方法
- 1.1.5 实现export_state_interfaces和export_command_interfaces方法
- 1.1.6 (可选)实现prepare_command_mode_switch和perform_commandmode_switch
- 1.1.7 实现on_activate方法
- 1.1.8 实现on_dectivate方法
- 1.1.9 实现on_shutdown方法
- 1.1.10 实现on_error方法
- 1.1.11 实现read方法
- 1.1.12 实现write方法
- 1.1.13 添加PLUGINILIB_EXPORT_CLASS宏
- 1.5 编写pluginlib导出定义
- 1.6 修改CMakeLists.txt
- 1.7 将依赖项添加到package.xml文件中
- 1.8 编译和测试
1. 编写Hardware Component代码
1.1 创建包
进入[ros工作空间目录]/src
使用ros2 pkg create
创建包。包应该具有ament_cmake作为构建类型。例如创建包my_hardware_interface
:
ros2 pkg create --build-type ament_cmake my_hardware_interface
1.2 创建源文件
使用ros_team_workspace
辅助创建文件
# 注:需要在环境变量中增加ROS_WS指向ros工作空间目录
git clone https://github.com/StoglRobotics/ros_team_workspace.git
source ros_team_workspace/setup.bash
cd ${ROS_WS}/src/my_hardware_interface
ros2_control_setup-hardware-interface-package rrbot_hardware_interface RRBotHardwareInterface
其中rrbot_hardware_interface
为文件名,RRBotHardwareInterface
为类名。结构如下图所示:
1.3 向头文件(.hpp)中添加声明
- 使用头文件保护
#ifndef
和#define
预处理器指令。 - Include
"hardware_interface/$interface_type$_interface.hpp"
,$interface_type$
可以是Actuator
、Sensor
或System
,具体取决于使用的硬件类型。 - 为硬件接口定义一个唯一的命名空间。通常用包名。
- 定义硬件接口类,
$InterfaceType$interface
,例如…
c++ class硬件接口名称:public hardware_interface::$InterfaceType$interface
- 添加一个不带参数的构造函数和以下实现
LifecycleNodeInterface
的public函数:on_configure
、on_cleap
、on_shutdown
、on_cactivate
、on_dectivate
、on_error
;重写$InterfaceType$
接口定义:on_init
、export_state_interfaces
、export_command_interfaces
,prepare_command_mode_switch
(可选)、perform_command_mode_switch
(可选),read
、write
。
1.4 在源文件(.cpp)中添加定义
1.1.1 Include 硬件接口的头文件,并添加 namespace 以简化开发。
1.1.2 实现on_init方法。
在这里初始化所有成员变量,并处理传入的info
参数。在第一行中,通常调用parent_init
来处理标准值,如name
。即使用了hardware_interface::(Actuator | Sensor | System)interface::on_init(info)
完成的。如果所有的参数都已设置且有效,则返回CallbackReturn::SUCCESS
,否则返回CallbackReturn::ERROR
。RRBot的例子如下:
hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init(
const hardware_interface::HardwareInfo &info)
{
// 调用父类的on_init函数,检查是否成功初始化系统接口
if (hardware_interface::SystemInterface::on_init(info) !=
hardware_interface::CallbackReturn::SUCCESS)
{
// 如果初始化失败,返回ERROR
return hardware_interface::CallbackReturn::ERROR;
}
// BEGIN: 以下这部分代码仅用于示例目的 - 请不要复制到生产代码中
// 从硬件参数中获取示例参数,这部分代码仅用于示例,不要复制到生产代码中
hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]);
// END: 以上这部分代码仅用于示例目的 - 请不要复制到生产代码中
// 初始化硬件状态和命令容器,使用NaN表示未初始化的状态
hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
// 遍历所有的关节,检查它们的接口配置是否正确
for (const hardware_interface::ComponentInfo &joint : info_.joints)
{
// 每个关节应该有一个状态接口和一个命令接口
if (joint.command_interfaces.size() != 1)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s'有%zu个命令接口,但期望只有1个。", joint.name.c_str(),
joint.command_interfaces.size());
return hardware_interface::CallbackReturn::ERROR;
}
// 检查命令接口是否是位置命令接口
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s'的命令接口是'%s',但期望是'%s'。", joint.name.c_str(),
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
return hardware_interface::CallbackReturn::ERROR;
}
// RRBotSystemPositionOnlyHardware每个关节应该有一个位置状态接口
if (joint.state_interfaces.size() != 1)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s'有%zu个状态接口,但期望只有1个。", joint.name.c_str(),
joint.state_interfaces.size());
return hardware_interface::CallbackReturn::ERROR;
}
// 检查状态接口是否是位置状态接口
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s'的状态接口是'%s',但期望是'%s'。", joint.name.c_str(),
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
return hardware_interface::CallbackReturn::ERROR;
}
}
// 初始化成功,返回SUCCESS
return hardware_interface::CallbackReturn::SUCCESS;
}
1.1.3 实现on_configure方法
在该方法中设置与硬件的通信,激活硬件。例子如下:
hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// BEGIN: 以下这部分代码仅用于示例目的 - 请不要复制到生产代码中
// 输出一些示例信息,用于配置时的演示目的,不要复制到生产代码中
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait...");
// 模拟配置过程,等待一段时间,仅用于示例目的
for (int i = 0; i < hw_start_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...",
hw_start_sec_ - i);
}
// END: 以上这部分代码仅用于示例目的 - 请不要复制到生产代码中
// 在配置硬件时,重置硬件状态和命令的值为0
for (uint i = 0; i < hw_states_.size(); i++)
{
hw_states_[i] = 0;
hw_commands_[i] = 0;
}
// 输出配置成功的信息
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!");
// 配置成功,返回SUCCESS
return hardware_interface::CallbackReturn::SUCCESS;
}
1.1.4 实现on_cleap
方法
其作用与on_configure
相反。
1.1.5 实现export_state_interfaces和export_command_interfaces方法
其中定义了硬件提供的接口。对于Sensor
类型的硬件接口,没有export_command_interfaces
方法。完整的接口名称应该具有结构<joint_name>/<interface_type>
。机械臂export_state_interfaces
例子:
std::vector<hardware_interface::StateInterface>
RRBotSystemMultiInterfaceHardware::export_state_interfaces()
{
// 创建一个空的状态接口容器,用于存储硬件接口的状态信息
std::vector<hardware_interface::StateInterface> state_interfaces;
// 遍历所有的关节,将它们的状态信息添加到state_interfaces容器中
for (std::size_t i = 0; i < info_.joints.size(); i++)
{
// 创建一个StateInterface对象,表示关节的位置信息,并将其添加到state_interfaces中
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i]));
// 创建一个StateInterface对象,表示关节的速度信息,并将其添加到state_interfaces中
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i]));
// 创建一个StateInterface对象,表示关节的加速度信息,并将其添加到state_interfaces中
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &hw_accelerations_[i]));
}
// 返回包含状态接口信息的state_interfaces容器
return state_interfaces;
}
机械臂export_command_interfaces
例子:
std::vector<hardware_interface::CommandInterface> RRBotSystemMultiInterfaceHardware::export_command_interfaces()
{
// 创建一个空的命令接口容器,用于存储硬件接口的命令信息
std::vector<hardware_interface::CommandInterface> command_interfaces;
// 遍历所有的关节,将它们的命令信息添加到command_interfaces容器中
for (std::size_t i = 0; i < info_.joints.size(); i++)
{
// 创建一个CommandInterface对象,表示关节的位置命令,并将其添加到command_interfaces中
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_positions_[i]));
// 创建一个CommandInterface对象,表示关节的速度命令,并将其添加到command_interfaces中
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i]));
// 创建一个CommandInterface对象,表示关节的加速度命令,并将其添加到command_interfaces中
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &hw_commands_accelerations_[i]));
}
// 返回包含命令接口信息的command_interfaces容器
return command_interfaces;
}
1.1.6 (可选)实现prepare_command_mode_switch和perform_commandmode_switch
对于Actuator
和System
类型的硬件接口,如果硬件要接受多种控制模式,则需要实现prepare_command_mode_switch
和perform_commandmode_switch
。
1.1.7 实现on_activate方法
启用硬件“电源”的时,对应实现on_activate方法。
hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Activating ...please wait...");
for (int i = 0; i < hw_start_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...",
hw_start_sec_ - i);
}
// END: This part here is for exemplary purposes - Please do not copy to your production code
// command and state should be equal when starting
for (uint i = 0; i < hw_states_.size(); i++)
{
hw_commands_[i] = hw_states_[i];
}
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!");
return hardware_interface::CallbackReturn::SUCCESS;
}
1.1.8 实现on_dectivate方法
该方法的作用与on_activate
相反。
hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Deactivating ...please wait...");
for (int i = 0; i < hw_stop_sec_; i++)
{
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...",
hw_stop_sec_ - i);
}
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully deactivated!");
// END: This part here is for exemplary purposes - Please do not copy to your production code
return hardware_interface::CallbackReturn::SUCCESS;
}
1.1.9 实现on_shutdown方法
对应硬件正常关闭,实现on_shutdown
方法。
1.1.10 实现on_error方法
处理错误。
1.1.11 实现read方法
用来获取硬件状态并将其存储到export_state_interfaces
中定义的内部变量中。
hardware_interface::return_type RRBotSystemPositionOnlyHardware::read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading...");
for (uint i = 0; i < hw_states_.size(); i++)
{
// Simulate RRBot's movement
hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!",
hw_states_[i], i);
}
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!");
// END: This part here is for exemplary purposes - Please do not copy to your production code
return hardware_interface::return_type::OK;
}
1.1.12 实现write方法
该方法根据export_command_interfaces
中定义的内部变量中存储的值来执行命令操纵硬件。
hardware_interface::return_type RRBotSystemPositionOnlyHardware::write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing...");
for (uint i = 0; i < hw_commands_.size(); i++)
{
// Simulate sending commands to the hardware
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!",
hw_commands_[i], i);
}
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!");
// END: This part here is for exemplary purposes - Please do not copy to your production code
return hardware_interface::return_type::OK;
}
1.1.13 添加PLUGINILIB_EXPORT_CLASS宏
在命名空间关闭后的文件末尾,添加PLUGINILIB_EXPORT_CLASS
宏。
PLUGINLIB_EXPORT_CLASS(
ros2_control_demo_example_1::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface)
1.5 编写pluginlib导出定义
- 在包中创建
<my_hardware_interface_package>.xml
文件,并添加库和硬件接口类的定义,该类必须对pluginlib可见。参考例子:
<library path="ros2_control_demo_example_1">
<class name="ros2_control_demo_example_1/RRBotSystemPositionOnlyHardware"
type="ros2_control_demo_example_1::RRBotSystemPositionOnlyHardware"
base_class_type="hardware_interface::SystemInterface">
<description>
The ros2_control RRbot example using a system hardware interface-type.
</description>
</class>
</library>
- 通常,插件名称由包(命名空间)和类名定义,例如
<my_hardware_interface_package>/<RobotHardwareInterfaceName>
。当资源管理器搜索硬件接口时,此名称定义硬件接口的类型。其他两个参数必须与<robot_hardware_interface_name>.cpp
文件底部宏中的定义相对应。
1.6 修改CMakeLists.txt
- 在
find_package(ament_cmake REQUIRED)
行下添加更多的依赖项。它们至少是:hardware_interface
、pluginlib
、rclcpp
和rclcpp_lifecycle
。
# find dependencies
set(THIS_PACKAGE_INCLUDE_DEPENDS
hardware_interface
pluginlib
rclcpp
rclcpp_lifecycle
)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()
- 为
<robot_hardware_interface_name>.cpp
文件作为源的共享库添加编译指令。
## 编译
add_library(
ros2_control_demo_example_1
SHARED
hardware/rrbot.cpp
)
- 添加targeted目标和include目录。通常只需要include目录。
target_include_directories(ros2_control_demo_example_1 PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
$<INSTALL_INTERFACE:include/ros2_control_demo_example_1>
)
- 添加库所需的辅助项。
ament_target_dependencies(
ros2_control_demo_example_1 PUBLIC
${THIS_PACKAGE_INCLUDE_DEPENDS}
)
# 设置目标(${PROJECT_NAME})的编译定义(宏),以控制可见性。
# 这个宏在目标编译时会被定义为 "ROS2_CONTROL_DEMO_EXAMPLE_1_BUILDING_DLL"。
# 用于构建动态链接库(DLL)时,控制导出符号的可见性。
target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_1_BUILDING_DLL")
- 使用以下命令导出pluginlib描述文件
pluginlib_export_plugin_description_file(hardware_interface <my_hardware_interface_package>.xml)
pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_1.xml)
- 为targets和include目录添加安装指令。
# 安装 'hardware/include/' 目录中的文件到指定目录 'include/ros2_control_demo_example_1'
install(
DIRECTORY hardware/include/
DESTINATION include/ros2_control_demo_example_1
)
# 安装 'description/launch', 'description/ros2_control', 和 'description/urdf' 目录中的文件
# 到指定目录 'share/ros2_control_demo_example_1'
install(
DIRECTORY description/launch description/ros2_control description/urdf
DESTINATION share/ros2_control_demo_example_1
)
# 安装 'bringup/launch' 和 'bringup/config' 目录中的文件到指定目录 'share/ros2_control_demo_example_1'
install(
DIRECTORY bringup/launch bringup/config
DESTINATION share/ros2_control_demo_example_1
)
# 安装生成的动态库或可执行文件到 'ros2_control_demo_example_1' 到指定目录中。
# 安装的位置依赖于目标的类型。
install(TARGETS ros2_control_demo_example_1
EXPORT export_ros2_control_demo_example_1
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
- (可选)在
ament_package()
之前将硬件接口的库添加到ament_export_libraries
中。
# 使用 ament_export_targets 定义要导出的目标(export_ros2_control_demo_example_1),
# 并指示该目标是否包含库目标。
ament_export_targets(export_ros2_control_demo_example_1 HAS_LIBRARY_TARGET)
# 使用 ament_export_dependencies 指定要导出的依赖项,通常是包含在 ${THIS_PACKAGE_INCLUDE_DEPENDS} 中。
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
# 使用 ament_package() 生成软件包信息,这包括软件包的名称、版本、作者、描述等。
ament_package()
1.7 将依赖项添加到package.xml文件中
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<exec_depend>controller_manager</exec_depend>
<exec_depend>forward_command_controller</exec_depend>
<exec_depend>joint_state_broadcaster</exec_depend>
<exec_depend>joint_state_publisher_gui</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>ros2_control_demo_description</exec_depend>
<exec_depend>ros2_controllers_test_nodes</exec_depend>
<exec_depend>ros2controlcli</exec_depend>
<exec_depend>ros2launch</exec_depend>
<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
1.8 编译和测试
- 进入工作区的根目录,使用
colcon build<my_hardware_interface_package>
命令编译硬件组件。 - 如果编译成功,在安装文件夹中执行
source setup.bash
文件,并执行colcon test<my_hardware_interface_package>
以检查是否可以通过pluginlib
库找到新控制器并由控制器管理器加载。