【ros2 control 機(jī)器人驅(qū)動(dòng)開發(fā)】簡(jiǎn)單雙關(guān)節(jié)機(jī)器人學(xué)習(xí)-example 1

這篇具有很好參考價(jià)值的文章主要介紹了【ros2 control 機(jī)器人驅(qū)動(dòng)開發(fā)】簡(jiǎn)單雙關(guān)節(jié)機(jī)器人學(xué)習(xí)-example 1。希望對(duì)大家有所幫助。如果存在錯(cuò)誤或未考慮完全的地方，請(qǐng)大家不吝賜教，您也可以點(diǎn)擊"舉報(bào)違法"按鈕提交疑問。

前言

本系列文件主要有以下目標(biāo)和內(nèi)容：

為系統(tǒng)、傳感器和執(zhí)行器創(chuàng)建 HardwareInterface
以URDF文件的形式創(chuàng)建機(jī)器人描述
加載配置并使用啟動(dòng)文件啟動(dòng)機(jī)器人
控制RRBot的兩個(gè)關(guān)節(jié)（兩旋轉(zhuǎn)關(guān)節(jié)機(jī)器人）
六自由度機(jī)器人的控制
實(shí)現(xiàn)機(jī)器人的控制器切換策略
使用 ros2_control 中的關(guān)節(jié)限制和傳輸概念

一、RR機(jī)器人

RRBot（ Revolute-Revolute Manipulator Robot）是一個(gè)簡(jiǎn)單的3連桿，2關(guān)節(jié)的機(jī)械臂，我們將使用它來演示各種功能。

它本質(zhì)上是一個(gè)雙倒立擺，并在模擬器中演示了一些有趣的控制概念，最初是為Gazebo教程介紹的。¹

創(chuàng)建description pkg

這里主要是完成機(jī)器人描述文件的創(chuàng)建，各個(gè)軸的物理尺寸、模型信息，各個(gè)軸的關(guān)節(jié)鏈接方式、鏈接點(diǎn)。

mkdir ~/ros2_control_demos
cd ~/ros2_control_demos

ros2 pkg create --build-type ament_cmake ros2_control_demo_description

# 文件結(jié)構(gòu)
$ tree ros2_control_demo_description/
ros2_control_demo_description/
├── CMakeLists.txt
├── package.xml
└── rrbot
    ├── rviz
    │   └── rrbot.rviz
    └── urdf
        ├── rrbot.materials.xacro
        └── rrbot_description.urdf.xacro

# CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(ros2_control_demo_description)

# find dependencies
find_package(ament_cmake REQUIRED)

install(
  DIRECTORY rrbot/urdf rrbot/rviz
  DESTINATION share/${PROJECT_NAME}/rrbot
)

ament_package()

# packages.xml

<?xml version="1.0"?>
<?xml-model  schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>ros2_control_demo_description</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="someone@example.com">Bing_Lee</maintainer>
  <url type="website">https://blog.csdn.net/Bing_Lee</url>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <exec_depend>joint_state_publisher_gui</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>rviz2</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

# rrbot.rviz

Panels:
  - Class: rviz_common/Displays
    Help Height: 87
    Name: Displays
    Property Tree Widget:
      Expanded: ~
      Splitter Ratio: 0.5
    Tree Height: 1096
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
    Expanded:
      - /2D Goal Pose1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz_common/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/RobotModel
      Collision Enabled: false
      Description File: ""
      Description Source: Topic
      Description Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /robot_description
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        camera_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        camera_link_optical:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        hokuyo_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link1:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        link2:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        tool_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        world:
          Alpha: 1
          Show Axes: false
          Show Trail: false
      Mass Properties:
        Inertia: false
        Mass: false
      Name: RobotModel
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: base_link
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz_default_plugins/Interact
      Hide Inactive Objects: true
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera
    - Class: rviz_default_plugins/Measure
      Line color: 128; 128; 0
    - Class: rviz_default_plugins/SetInitialPose
      Covariance x: 0.25
      Covariance y: 0.25
      Covariance yaw: 0.06853891909122467
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /initialpose
    - Class: rviz_default_plugins/SetGoal
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /goal_pose
    - Class: rviz_default_plugins/PublishPoint
      Single click: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /clicked_point
  Transformation:
    Current:
      Class: rviz_default_plugins/TF
  Value: true
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
      Distance: 8.443930625915527
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0.0044944556429982185
        Y: 1.0785865783691406
        Z: 2.4839563369750977
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.23039916157722473
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 5.150422096252441
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 1379
  Hide Left Dock: false
  Hide Right Dock: false
  Selection:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
  Width: 2560
  X: 0
  Y: 1470

# rrbot_description.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="rrbot" params="parent prefix *origin">

  <!-- Constants for robot dimensions -->
  <xacro:property name="mass" value="1" /> <!-- arbitrary value for mass -->
  <xacro:property name="width" value="0.1" /> <!-- Square dimensions (widthxwidth) of beams -->
  <xacro:property name="height1" value="2" /> <!-- Link 1 -->
  <xacro:property name="height2" value="1" /> <!-- Link 2 -->
  <xacro:property name="height3" value="1" /> <!-- Link 3 -->
  <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->

  <joint name="${prefix}base_joint" type="fixed">
    <xacro:insert_block name="origin" />
    <parent link="${parent}"/>
    <child link="${prefix}base_link" />
  </joint>

  <!-- Base Link -->
  <link name="${prefix}base_link">
    <collision>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height1}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height1}"/>
      </geometry>
      <material name="orange"/>
    </visual>
  </link>

  <joint name="${prefix}joint1" type="continuous">
    <parent link="${prefix}base_link"/>
    <child link="${prefix}link1"/>
    <origin xyz="0 ${width} ${height1 - axel_offset}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.7"/>
  </joint>

  <!-- Middle Link -->
  <link name="${prefix}link1">
    <collision>
      <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height2}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height2}"/>
      </geometry>
      <material name="yellow"/>
    </visual>
  </link>

  <joint name="${prefix}joint2" type="continuous">
    <parent link="${prefix}link1"/>
    <child link="${prefix}link2"/>
    <origin xyz="0 ${width} ${height2 - axel_offset*2}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.7"/>
  </joint>

  <!-- Top Link -->
  <link name="${prefix}link2">
    <collision>
      <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height3}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
      <geometry>
        <box size="${width} ${width} ${height3}"/>
      </geometry>
      <material name="orange"/>
    </visual>
  </link>

  <joint name="${prefix}tool_joint" type="fixed">
    <origin xyz="0 0 1" rpy="0 0 0" />
    <parent link="${prefix}link2"/>
    <child link="${prefix}tool_link" />
  </joint>

  <!-- Tool Link -->
  <link name="${prefix}tool_link">
  </link>

  </xacro:macro>

</robot>

# rrbot.materials.xacro
<?xml version="1.0"?>
<!--
Copied from ROS1 example -
https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/materials.xacro
-->
<robot>

  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>

  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>

  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>

  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>

  <material name="orange">
    <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
  </material>

  <material name="brown">
    <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
  </material>

  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>

  <material name="yellow">
    <color rgba="1.0 1.0 0.0 1.0"/>
  </material>

  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>

</robot>

創(chuàng)建demos pkg

這個(gè)包用于引導(dǎo)編譯所有相關(guān)依賴包，按照下邊格式填好即可。

cd ~/ros2_control_demos

ros2 pkg create --build-type ament_cmake ros2_control_demos

# 文件結(jié)構(gòu)
$ tree ros2_control_demos/
ros2_control_demos/
├── CMakeLists.txt
└── package.xml

# CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(ros2_control_demos)

# find dependencies
find_package(ament_cmake REQUIRED)

ament_package()

# package.xml

<?xml version="1.0"?>
<?xml-model  schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>ros2_control_demos</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="someone@example.com">Bing_Lee</maintainer>
  <url type="website">https://blog.csdn.net/Bing_Lee</url>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <exec_depend>ros2_control_demo_example_1</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

二、創(chuàng)建controller相關(guān)

創(chuàng)建example pkg

cd ~/ros2_control_demos

ros2 pkg create --build-type ament_cmake ros2_control_demo_example_1

這里涉及hardware部分的實(shí)現(xiàn)，具體如下：

# rrobot.hpp
// Copyright 2020 ros2_control Development Team
//
// 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 ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_
#define ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "ros2_control_demo_example_1/visibility_control.h"

namespace ros2_control_demo_example_1
{
class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface
{
public:
  RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware)

  // 從機(jī)器人的URDF解析的數(shù)據(jù)中解析硬件接口.
  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;

  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  hardware_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

  // 導(dǎo)出此硬件接口的所有狀態(tài)接口.
  // 必須根據(jù)為配置傳入的硬件信息創(chuàng)建和傳輸狀態(tài)接口.
  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

  // 導(dǎo)出此硬件接口的所有命令接口.
  // 命令接口必須根據(jù)為配置傳入的硬件信息創(chuàng)建和傳輸.
  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

  // 從執(zhí)行器讀取當(dāng)前狀態(tài)值.
  // 使用接口讀取硬件當(dāng)前狀態(tài)，注意硬件狀態(tài)應(yīng)該是實(shí)時(shí)更新的.
  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  // 將當(dāng)前指令值寫入執(zhí)行器.
  // 使用接口將最新值設(shè)置到硬件中.
  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  hardware_interface::return_type write(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:
  // Parameters for the RRBot simulation
  double hw_start_sec_;
  double hw_stop_sec_;
  double hw_slowdown_;

  // Store the command for the simulated robot
  std::vector<double> hw_commands_;
  std::vector<double> hw_states_;
};

}  // namespace ros2_control_demo_example_1

#endif  // ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_

// Copyright 2021 ros2_control Development Team
//
// 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.

/* This header must be included by all rclcpp headers which declare symbols
 * which are defined in the rclcpp library. When not building the rclcpp
 * library, i.e. when using the headers in other package's code, the contents
 * of this header change the visibility of certain symbols which the rclcpp
 * library cannot have, but the consuming code must have inorder to link.
 */

#ifndef ROS2_CONTROL_DEMO_EXAMPLE_1__VISIBILITY_CONTROL_H_
#define ROS2_CONTROL_DEMO_EXAMPLE_1__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 ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __attribute__((dllexport))
#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT __attribute__((dllimport))
#else
#define ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __declspec(dllexport)
#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT __declspec(dllimport)
#endif
#ifdef ROS2_CONTROL_DEMO_EXAMPLE_1_BUILDING_DLL
#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT
#else
#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT
#endif
#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL
#else
#define ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __attribute__((visibility("default")))
#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT
#if __GNUC__ >= 4
#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC __attribute__((visibility("default")))
#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL __attribute__((visibility("hidden")))
#else
#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL
#endif
#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC_TYPE
#endif

#endif  // ROS2_CONTROL_DEMO_EXAMPLE_1__VISIBILITY_CONTROL_H_

attribute((visibility(“default”))) 是 GCC（GNU 編譯器集合）中的一個(gè)特性，它允許程序員控制特定部分的代碼的可見性。在 C 和 C++ 中，代碼的可見性是指其他源文件能否訪問特定的函數(shù)或變量。

當(dāng)你在一個(gè)函數(shù)或變量聲明中使用 attribute((visibility(“default”)))，表示這個(gè)函數(shù)或變量默認(rèn)對(duì)所有其他源文件可見。也就是說，這個(gè)函數(shù)或變量可以在其他源文件中被調(diào)用，或者被其他源文件中的變量間接引用。

這種特性在編寫庫(kù)和模塊時(shí)非常有用，因?yàn)樗试S程序員更靈活地組織和隱藏代碼。例如，你可以創(chuàng)建一個(gè)庫(kù)，其中包含一些默認(rèn)可見的函數(shù)和變量，這些函數(shù)和變量可以被其他源文件調(diào)用，但不被直接暴露給用戶。這種方式可以隱藏庫(kù)的內(nèi)部實(shí)現(xiàn)細(xì)節(jié)，同時(shí)仍然允許用戶使用庫(kù)的功能。

需要注意的是，attribute((visibility(“default”))) 并不是 C 或 C++ 標(biāo)準(zhǔn)的一部分，而是 GCC 編譯器的一個(gè)特性。這意味著不是所有的編譯器都會(huì)支持這個(gè)特性，特別是那些不支持 GCC 的編譯器。此外，不同的編譯器可能對(duì) attribute((visibility)) 的行為有微小的差異，所以在使用這個(gè)特性時(shí)需要特別小心。

# robot.cpp

// Copyright 2020 ros2_control Development Team
//
// 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 "ros2_control_demo_example_1/rrbot.hpp"

#include <chrono>
#include <cmath>
#include <limits>
#include <memory>
#include <vector>

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

namespace ros2_control_demo_example_1
{
hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (
    hardware_interface::SystemInterface::on_init(info) !=
    hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  // BEGIN: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境
  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: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境

  hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

  // 校驗(yàn)狀態(tài)和命令接口是否只有一個(gè)
  for (const hardware_interface::ComponentInfo & joint : info_.joints)
  {
    // RRBotSystemPositionOnly 只有一種狀態(tài)和命令(state,command)
    if (joint.command_interfaces.size() != 1)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
        "Joint '%s' has %zu command interfaces found. 1 expected.", 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' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces.size() != 1)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
        "Joint '%s' has %zu state interface. 1 expected.", 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' have %s state interface. '%s' expected.", joint.name.c_str(),
        joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
      return hardware_interface::CallbackReturn::ERROR;
    }
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // BEGIN: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境
  RCLCPP_INFO(
    rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait...");

  // configure 前延時(shí)
  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: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境

  // 在切換狀態(tài)到configure時(shí)總是初始化所有值
  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!");

  return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface>
RRBotSystemPositionOnlyHardware::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++)
  {
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
RRBotSystemPositionOnlyHardware::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++)
  {
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
  }

  return command_interfaces;
}

hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // BEGIN: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境
  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: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境

  // command和state開始時(shí)應(yīng)該是相同的
  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;
}

hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // BEGIN: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境
  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: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type RRBotSystemPositionOnlyHardware::read(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // BEGIN: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境
  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading...");

  for (uint i = 0; i < hw_states_.size(); i++)
  {
    // 模擬RRBot的運(yùn)動(dòng)，這一塊是延時(shí)運(yùn)動(dòng)更新state逐步靠近c(diǎn)ommand位置
    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: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type RRBotSystemPositionOnlyHardware::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // BEGIN: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境
  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing...");

  for (uint i = 0; i < hw_commands_.size(); i++)
  {
    // 仿真發(fā)送命令到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: 這一段是出測(cè)試使用考慮，請(qǐng)勿拷貝至實(shí)際生產(chǎn)環(huán)境

  return hardware_interface::return_type::OK;
}

}  // namespace ros2_control_demo_example_1

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  ros2_control_demo_example_1::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface)

創(chuàng)建bringup

cd ~/ros2_control_demos/ros2_control_demo_example_1
tree bringup/
bringup/
├── config
│   ├── rrbot_controllers.yaml
│   ├── rrbot_forward_position_publisher.yaml
│   └── rrbot_joint_trajectory_publisher.yaml
└── launch
    ├── rrbot.launch.py
    ├── test_forward_position_controller.launch.py
    └── test_joint_trajectory_controller.launch.py

2 directories, 6 files

rrbot_controllers.yaml定義controller總體參數(shù)屬性和接口名稱

# rrbot_controllers.yaml

controller_manager:
  ros__parameters:
    update_rate: 10  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    forward_position_controller:
      type: forward_command_controller/ForwardCommandController

    joint_trajectory_position_controller:
      type: joint_trajectory_controller/JointTrajectoryController


forward_position_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
    interface_name: position


joint_trajectory_position_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2

    command_interfaces:
      - position

    state_interfaces:
      - position

    action_monitor_rate: 20.0 # Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    open_loop_control: true
    allow_integration_in_goal_trajectories: true
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)

launch文件，負(fù)責(zé)啟動(dòng)controller_manager、robot_state_publisher等

# rrbot.launch.py

# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschr?nkt)
#
# 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.


from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # Declare arguments
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument(
            "gui",
            default_value="true",
            description="Start RViz2 automatically with this launch file.",
        )
    )

    # Initialize Arguments
    gui = LaunchConfiguration("gui")

    # Get URDF via xacro
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [
                    FindPackageShare("ros2_control_demo_example_1"),
                    "urdf",
                    "rrbot.urdf.xacro",
                ]
            ),
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    robot_controllers = PathJoinSubstitution(
        [
            FindPackageShare("ros2_control_demo_example_1"),
            "config",
            "rrbot_controllers.yaml",
        ]
    )
    rviz_config_file = PathJoinSubstitution(
        [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"]
    )

    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description, robot_controllers],
        output="both",
    )
    robot_state_pub_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description],
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        condition=IfCondition(gui),
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
    )

    robot_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["forward_position_controller", "--controller-manager", "/controller_manager"],
    )

    # Delay rviz start after `joint_state_broadcaster`
    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=joint_state_broadcaster_spawner,
            on_exit=[rviz_node],
        )
    )

    # Delay start of robot_controller after `joint_state_broadcaster`
    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=joint_state_broadcaster_spawner,
            on_exit=[robot_controller_spawner],
        )
    )

    nodes = [
        control_node,
        robot_state_pub_node,
        joint_state_broadcaster_spawner,
        delay_rviz_after_joint_state_broadcaster_spawner,
        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
    ]

    return LaunchDescription(declared_arguments + nodes)

# 運(yùn)行l(wèi)aunch文件
ros2 launch ros2_control_demo_example_1 rrbot.launch.py

# 打印當(dāng)前運(yùn)行節(jié)點(diǎn)
ros2 node list

# 輸出
/controller_manager
/forward_position_controller
/joint_state_broadcaster
/robot_state_publisher

hardware_interface::SystemInterface類的說明：²

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface類的說明：³

也可通過上一篇博客《 ROS2 LifecycleNode講解及實(shí)例》中的介紹學(xué)習(xí)。

【ros2 control 機(jī)器人驅(qū)動(dòng)開發(fā)】簡(jiǎn)單雙關(guān)節(jié)機(jī)器人學(xué)習(xí)-example 1,ROS2機(jī)器人操作系統(tǒng),機(jī)器人,人工智能

三、測(cè)試運(yùn)行

運(yùn)行主程序：

# 編譯項(xiàng)目
colcon build --packages-up-to ros2_control_demos

# 運(yùn)行項(xiàng)目
ros2 launch ros2_control_demo_example_1 rrbot.launch.py

新啟動(dòng)一個(gè)窗口，查看當(dāng)前controller狀態(tài)：

# 查看當(dāng)前controller狀態(tài)
ros2 control list_controllers

# -----------OUT PUT-----------------
forward_position_controller[forward_command_controller/ForwardCommandController] active    
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active

測(cè)試forward_command_controller

可以看到forward_command_controller和joint_state_broadcaster兩個(gè)處于active狀態(tài)，我們先測(cè)試forward_command_controller，運(yùn)行命令如下：

# 這里會(huì)每隔8s測(cè)試性輸出一個(gè)指令
ros2 launch ros2_control_demo_example_1 test_forward_position_controller.launch.py

運(yùn)行結(jié)果如下：
【ros2 control 機(jī)器人驅(qū)動(dòng)開發(fā)】簡(jiǎn)單雙關(guān)節(jié)機(jī)器人學(xué)習(xí)-example 1,ROS2機(jī)器人操作系統(tǒng),機(jī)器人,人工智能

測(cè)試joint_trajectory_controller

ctrl+c停止前一個(gè)窗口

# 加載joint_trajectory_controller
ros2 control load_controller joint_trajectory_position_controller --set-state inactive

# 切換controller使用狀態(tài)
ros2 control set_controller_state forward_position_controller inactive
ros2 control set_controller_state joint_trajectory_position_controller active

# 查看當(dāng)前controller狀態(tài)
ros2 control list_controllers

# -----------OUT PUT-----------------
joint_trajectory_position_controller[joint_trajectory_controller/JointTrajectoryController] active    
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active    
forward_position_controller[forward_command_controller/ForwardCommandController] inactive

運(yùn)行測(cè)試指令：

ros2 launch ros2_control_demo_example_1 test_joint_trajectory_controller.launch.py

運(yùn)行結(jié)果如下：

總結(jié)

這里就是example1的整體解說了，對(duì)于controller及其定義以及如何調(diào)試運(yùn)行都做了說明，整個(gè)項(xiàng)目代碼避免有小伙伴寫錯(cuò)已上傳至gitee代碼庫(kù)，地址如下**Bing Lee / Learn Ros2 Moveit**，請(qǐng)期待下一篇此系列文章。

Example 1: RRBot — ROS2_Control: Rolling Dec 2023 documentation ??
ROS2 Control: hardware_interface::SystemInterface Class Reference ??
Class LifecycleNodeInterface ??文章來源地址http://www.zghlxwxcb.cn/news/detail-762559.html

到了這里，關(guān)于【ros2 control 機(jī)器人驅(qū)動(dòng)開發(fā)】簡(jiǎn)單雙關(guān)節(jié)機(jī)器人學(xué)習(xí)-example 1的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！