【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
<package format="3">
<name>ros2_control_demo_descriptionname>
<version>0.0.0version>
<description>TODO: Package descriptiondescription>
<maintainer email="https://blog.csdn.net/Bing_Lee">devmaintainer>
<license>TODO: License declarationlicense>
<buildtool_depend>ament_cmakebuildtool_depend>
<exec_depend>joint_state_publisher_guiexec_depend>
<exec_depend>robot_state_publisherexec_depend>
<exec_depend>rviz2exec_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
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="rrbot" params="parent prefix *origin">
<xacro:property name="mass" value="1" />
<xacro:property name="width" value="0.1" />
<xacro:property name="height1" value="2" />
<xacro:property name="height2" value="1" />
<xacro:property name="height3" value="1" />
<xacro:property name="axel_offset" value="0.05" />
<joint name="${prefix}base_joint" type="fixed">
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
<child link="${prefix}base_link" />
joint>
<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>
<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>
<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>
<link name="${prefix}tool_link">
link>
xacro:macro>
robot>
# rrbot.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
<package format="3">
<name>ros2_control_demosname>
<version>0.0.0version>
<description>TODO: Package descriptiondescription>
<maintainer email="https://blog.csdn.net/Bing_Lee">devmaintainer>
<license>TODO: License declarationlicense>
<buildtool_depend>ament_cmakebuildtool_depend>
<exec_depend>ros2_control_demo_example_1exec_depend>
<export>
<build_type>ament_cmakebuild_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
#include
#include
#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 export_state_interfaces() override;
ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
std::vector 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 hw_commands_;
std::vector 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
#include
#include
#include
#include
#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::quiet_NaN());
hw_commands_.resize(info_.joints.size(), std::numeric_limits::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
RRBotSystemPositionOnlyHardware::export_state_interfaces()
{
std::vector 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
RRBotSystemPositionOnlyHardware::export_command_interfaces()
{
std::vector 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讲解及实例》中的介绍学习。
三、测试运行
总结
先把当前完成部分更新到博客，运行结果截图如上所示，这两天继续完善。
ROS2 Control: hardware_interface::SystemInterface Class Reference ↩︎
Class LifecycleNodeInterface ↩︎