ros2_control教程(2)

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）中添加声明

1. 使用头文件保护#ifndef和#define预处理器指令。
2. Include "hardware_interface/$interface_type$_interface.hpp"，$interface_type$可以是Actuator、Sensor或System，具体取决于使用的硬件类型。
3. 为硬件接口定义一个唯一的命名空间。通常用包名。
4. 定义硬件接口类，$InterfaceType$interface，例如…
   c++ class硬件接口名称：public hardware_interface::$InterfaceType$interface
5. 添加一个不带参数的构造函数和以下实现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导出定义

1. 在包中创建<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>

2. 通常，插件名称由包（命名空间）和类名定义，例如<my_hardware_interface_package>/<RobotHardwareInterfaceName>。当资源管理器搜索硬件接口时，此名称定义硬件接口的类型。其他两个参数必须与<robot_hardware_interface_name>.cpp文件底部宏中的定义相对应。

1.6 修改CMakeLists.txt

1. 在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()

2. 为<robot_hardware_interface_name>.cpp文件作为源的共享库添加编译指令。

## 编译
add_library(
  ros2_control_demo_example_1
  SHARED
  hardware/rrbot.cpp
)

3. 添加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>
)

4. 添加库所需的辅助项。

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")

5. 使用以下命令导出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)

6. 为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
)

7. （可选）在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 编译和测试

1. 进入工作区的根目录，使用colcon build＜my_hardware_interface_package＞命令编译硬件组件。
2. 如果编译成功，在安装文件夹中执行source setup.bash文件，并执行colcon test＜my_hardware_interface_package＞以检查是否可以通过pluginlib库找到新控制器并由控制器管理器加载。