【ros2 control 机器人驱动开发】简单双关节机器人学习-example 1

【ros2 control 机器人驱动开发】简单双关节机器人学习-example 1

文章目录

  • 前言
  • 一、RR机器人
    • 创建description pkg
    • 创建demos pkg
  • 二、创建controller相关
    • 创建example pkg
  • 三、测试运行
  • 总结

前言

本系列文件主要有以下目标和内容：

  • 为系统、传感器和执行器创建 HardwareInterface
  • 以URDF文件的形式创建机器人描述
  • 加载配置并使用启动文件启动机器人
  • 控制RRBot的两个关节（两旋转关节机器人）
  • 六自由度机器人的控制
  • 实现机器人的控制器切换策略
  • 使用 ros2_control 中的关节限制和传输概念

一、RR机器人

RRBot（ Revolute-Revolute Manipulator Robot）是一个简单的3连杆，2关节的机械臂，我们将使用它来演示各种功能。

它本质上是一个双倒立摆，并在模拟器中演示了一些有趣的控制概念，最初是为Gazebo教程介绍的。

创建description pkg

这里主要是完成机器人描述文件的创建，各个轴的物理尺寸、模型信息，各个轴的关节链接方式、链接点。

mkdir ~/ros2_control_demos
cd ~/ros2_control_demos

ros2 pkg create --build-type ament_cmake ros2_control_demo_description

# 文件结构
$ 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)

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)

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

ament_package()

# packages.xml

<?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>ros2_control_demo_description</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="https://blog.csdn.net/Bing_Lee">dev</maintainer>
  <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>
</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>

创建demos pkg

这个包用于引导编译所有相关依赖包，按照下边格式填好即可。

cd ~/ros2_control_demos

ros2 pkg create --build-type ament_cmake ros2_control_demos

# 文件结构
$ 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 href="http://download.ros.org/schema/package_format3.xsd" 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="https://blog.csdn.net/Bing_Lee">dev</maintainer>
  <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>

二、创建controller相关

创建example pkg

cd ~/ros2_control_demos

ros2 pkg create --build-type ament_cmake ros2_control_demo_example_1

这里涉及hardware部分的实现，具体如下：

# robot.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);

  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;

  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

  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;

  ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  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 编译器集合）中的一个特性，它允许程序员控制特定部分的代码的可见性。在 C 和 C++ 中，代码的可见性是指其他源文件能否访问特定的函数或变量。

当你在一个函数或变量声明中使用 attribute((visibility(“default”)))，表示这个函数或变量默认对所有其他源文件可见。也就是说，这个函数或变量可以在其他源文件中被调用，或者被其他源文件中的变量间接引用。

这种特性在编写库和模块时非常有用，因为它允许程序员更灵活地组织和隐藏代码。例如，你可以创建一个库，其中包含一些默认可见的函数和变量，这些函数和变量可以被其他源文件调用，但不被直接暴露给用户。这种方式可以隐藏库的内部实现细节，同时仍然允许用户使用库的功能。

需要注意的是，attribute((visibility(“default”))) 并不是 C 或 C++ 标准的一部分，而是 GCC 编译器的一个特性。这意味着不是所有的编译器都会支持这个特性，特别是那些不支持 GCC 的编译器。此外，不同的编译器可能对 attribute((visibility)) 的行为有微小的差异，所以在使用这个特性时需要特别小心。

# 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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境
  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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境

  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)
  {
    // RRBotSystemPositionOnly 只有一种状态和命令(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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境
  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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境

  // 在切换状态到configure时总是初始化所有值
  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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境
  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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境

  // command和state开始时应该是相同的
  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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境
  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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type RRBotSystemPositionOnlyHardware::read(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // BEGIN: 这一段是出测试使用考虑，请勿拷贝至实际生产环境
  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading...");

  for (uint i = 0; i < hw_states_.size(); i++)
  {
    // 模拟RRBot的运动
    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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type RRBotSystemPositionOnlyHardware::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // BEGIN: 这一段是出测试使用考虑，请勿拷贝至实际生产环境
  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing...");

  for (uint i = 0; i < hw_commands_.size(); i++)
  {
    // 仿真发送命令到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: 这一段是出测试使用考虑，请勿拷贝至实际生产环境

  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)

hardware_interface::SystemInterface类的说明：1

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface类的说明：2

也可通过上一篇博客《 ROS2 LifecycleNode讲解及实例》中的介绍学习。

在这里插入图片描述

在这里插入图片描述

三、测试运行

在这里插入图片描述

总结

先把当前完成部分更新到博客，运行结果截图如上所示，这两天继续完善。

  1. ROS2 Control: hardware_interface::SystemInterface Class Reference ↩︎

  2. Class LifecycleNodeInterface ↩︎

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/255987.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

Amazon CodeWhisperer 在 vscode 的应用

Amazon CodeWhisperer 在 vscode 的应用

文章作者:旧花阴 CodeWhisperer 是一款可以帮助程序员更快、更安全地编写代码的工具&#xff0c;可以在他们的开发环境中实时提供代码建议和推荐。亚马逊云科技发布的这款代码生成工具 CodeWhisperer 最大的优势就是对于个人用户免费。以在 vscode 为例&#xff0c;演示安装过程…
阅读更多...
Python tkinter控件全集之组合选择框 ttk.ComboBox

Python tkinter控件全集之组合选择框 ttk.ComboBox

Tkinter标准库 Tkinter是Python的标准GUI库&#xff0c;也是最常用的Python GUI库之一&#xff0c;提供了丰富的组件和功能&#xff0c;包括窗口、按钮、标签、文本框、列表框、滚动条、画布、菜单等&#xff0c;方便开发者进行图形界面的开发。Tkinter库基于Tk for Unix/Wind…
阅读更多...
【Image】图像处理

【Image】图像处理

计算机视觉 CV Perception 如自动驾驶领域。 只要是从所谓的图像当中去抽取信息的过程&#xff0c;我们都叫做Perception。 视觉检测可以涵盖二维检测&#xff0c;如车辆、人和信号灯的检测。另外&#xff0c;还可以控制三维信息&#xff0c;直接在三维空间中操作数据。 SL…
阅读更多...
IDEA新建jdk8 spring boot项目

IDEA新建jdk8 spring boot项目

今天新建spring boot项目发现JDK版本最低可选17。 但是目前用的最多的还是JDK8啊。 解决办法 Server URL中设置&#xff1a; https://start.aliyun.com/设置完成后&#xff0c;又可以愉快的用jdk8创建项目了。 打包的jar无法运行 小伙伴可能会发现&#xff0c;默认的配置打…
阅读更多...
9、ble_mesh基础

9、ble_mesh基础

node节点&#xff0c;不属于网络的设备称为未配置设备。未配置的设备无法发送或接收网格消息&#xff1b;但是&#xff0c;它会向 Provisioners 宣传其存在。 Provisioner供应&#xff0c;验证&#xff0c;邀请&#xff0c;加入网络成为节点。 一个节点有多个控制或开关&#x…
阅读更多...
四通道 DMOS 全桥驱动MS35631N/MS35631

四通道 DMOS 全桥驱动MS35631N/MS35631

MS35631N/MS35631 是一款四通道 DMOS 全桥驱动器&#xff0c;可以驱动两 个步进电机或者四个直流电机。每个全桥的驱动电流在 24V 电源下可以 达到 1.2A 。 MS35631N/MS35631 集成了固定关断时间的 PWM 电流校正 器&#xff0c;以及一个 2bit 的非线性 DAC &…
阅读更多...
美颜SDK技术对比，深入了解视频美颜SDK的工作机制

美颜SDK技术对比，深入了解视频美颜SDK的工作机制

如何在实时视频中呈现更加自然、美丽的画面&#xff0c;而这正是美颜SDK技术发挥作用的领域之一。本文将对几种主流视频美颜SDK进行深入比较&#xff0c;以揭示它们的工作机制及各自的优劣之处。 随着科技的不断进步&#xff0c;美颜技术已经从简单的图片处理发展到了视频领域…
阅读更多...
Vue3-22-组件-插槽的使用详解

Vue3-22-组件-插槽的使用详解

插槽是干啥的 插槽 就是 组件中的一个 占位符&#xff0c; 这个占位符 可以接收 父组件 传递过来的 html 的模板值&#xff0c;然后进行填充渲染。 就这么简单&#xff0c;插槽就是干这个的。要说它的优点吧&#xff0c;基本上就是可以使子组件的内容可以被父组件控制&#xf…
阅读更多...
商业办公楼烟雾预警，这个方法我只说一次！

商业办公楼烟雾预警，这个方法我只说一次！

烟感监控在现代社会中扮演着至关重要的角色&#xff0c;不仅在火灾预防和安全管理中具有关键性的作用&#xff0c;同时也对人们的生命财产安全起到了至关重要的保护作用。 随着科技的不断发展&#xff0c;烟感监控系统的功能日益强大&#xff0c;为各行业提供了更加智能、高效的…
阅读更多...
python图像二值化处理

python图像二值化处理

目录 1、双峰法 2、P参数法 3、迭代法 4、OTSU法 图像的二值化处理是将图像上的像素点的灰度值设置为0或255&#xff0c;也就是将整个图像呈现出明显的只有黑和白的视觉效果。二值化是图像分割的一种最简单的方法&#xff0c;可以把灰度图像转换成二值图像。具体实现是将大…
阅读更多...
lambda自定义比较规则-sort函数或优先队列

lambda自定义比较规则-sort函数或优先队列

Lambda表达式的一般形式为 [captures](params){body}对于优先队列的自定义排序规则&#xff0c;常见方法是写成结构体形式 struct cmp{bool operator()(pair<int,int> map1,pair<int,int> map2){return map1.second>map2.second;} }; priority_queue<pair&…
阅读更多...
electron与cesium组件入门应用功能

electron与cesium组件入门应用功能

electron与cesium组件入门应用功能 运行应用效果图&#xff1a; electron应用目录&#xff0c;需要包括三个文件: index.html main.js package.json (一)、创建一个新项目 目录名称&#xff1a;project_helloWolrd (二)、生成package.json文件 npm init --yes(三&#x…
阅读更多...
饥荒Mod 开发(十六)：五格装备栏

饥荒Mod 开发(十六)：五格装备栏

饥荒Mod 开发(十五)&#xff1a;小地图显示物品 源码 饥荒中的装备栏只有3个实在太少了&#xff0c;手&#xff0c;头&#xff0c;身体。 身体上装备的物品会有冲突&#xff0c;很多不能一起装备&#xff0c;比如 衣服&#xff0c;项链&#xff0c;背包等。 而这三种物品又有自…
阅读更多...
概率论复习

概率论复习

第一章&#xff1a;随机概率及其概率 A和B相容就是 AB 空集 全概率公式与贝叶斯公式&#xff1a; 伯努利求概率&#xff1a; 第二章&#xff1a;一维随机变量及其分布&#xff1a; 离散型随机变量求分布律&#xff1a; 利用常规离散性分布求概率&#xff1a; 连续性随机变量…
阅读更多...
Docker单点部署[8.11.3] Elasticsearch + Kibana + ik分词器

Docker单点部署[8.11.3] Elasticsearch + Kibana + ik分词器

文章目录 一、Elasticsearch二、Kibana三、访问四、其他五、ik分词器第一种&#xff1a;在线安装第二种&#xff1a;离线安装 Elasticsearch 和 Kibana 版本一般需要保持一致才能一起使用&#xff0c;但是从 8.x.x开始&#xff0c;安全验证不断加强&#xff0c;甚至8.x.x之间…
阅读更多...
Linux - 非root用户使用systemctl管理服务

Linux - 非root用户使用systemctl管理服务

文章目录 方式一 &#xff08;推荐&#xff09;1. 编辑sudoers文件&#xff1a;2. 设置服务文件权限&#xff1a;3. 启动和停止服务&#xff1a; 方式二1. 查看可用服务&#xff1a;2. 选择要配置的服务&#xff1a;3. 创建自定义服务文件&#xff1a;4. 重新加载systemd管理的…
阅读更多...
安防监控视频管理平台EasyCVR v3.4版如何取消首次登录强制重置密码的操作?

安防监控视频管理平台EasyCVR v3.4版如何取消首次登录强制重置密码的操作?

在视频监控领域&#xff0c;智慧安防平台EasyCVR平台采用了开放式的网络结构&#xff0c;支持高清视频的接入和传输、分发&#xff0c;能提供实时远程视频监控、视频录像、录像回放与存储、告警、语音对讲、云台控制、平台级联、磁盘阵列存储、视频集中存储、云存储等丰富的视频…
阅读更多...
【Spring】12 EmbeddedValueResolverAware 接口

【Spring】12 EmbeddedValueResolverAware 接口

文章目录 1. 简介2. 作用3. 使用3.1 创建并实现接口3.2 配置 Bean3.3 创建启动类3.4 启动 4. 应用场景总结 Spring 框架提供了许多回调接口&#xff0c;以便开发者在 Bean 的生命周期中执行一些特定操作。其中之一是 EmbeddedValueResolverAware 接口&#xff0c;本文将深入探…
阅读更多...
【Qt之Quick模块】2.创建Qt Quick UI工程

【Qt之Quick模块】2.创建Qt Quick UI工程

前言 上个文档对Qucik模块进行了概述&#xff0c;及创建Quick应用程序流程。 这个文档是创建Quick UI工程。 Qt Quick UI原型 Qt Quick UI原型项目对于测试或原型化用户界面非常有用&#xff0c;或者只是为了进行QML编辑而设置一个单独的项目。但是不能用它们进行应用程序开…
阅读更多...
智能优化算法应用：基于蛾群算法3D无线传感器网络(WSN)覆盖优化 - 附代码

智能优化算法应用：基于蛾群算法3D无线传感器网络(WSN)覆盖优化 - 附代码

智能优化算法应用&#xff1a;基于蛾群算法3D无线传感器网络(WSN)覆盖优化 - 附代码 文章目录 智能优化算法应用&#xff1a;基于蛾群算法3D无线传感器网络(WSN)覆盖优化 - 附代码1.无线传感网络节点模型2.覆盖数学模型及分析3.蛾群算法4.实验参数设定5.算法结果6.参考文献7.MA…
阅读更多...
最新文章

Copyright @ 2022~2023