广州在线网站制作推荐,沈阳优化网站公司,wordpress视屏站,优化关键词排名公司【ros2 control 机器人驱动开发】简单双关节机器人学习-example 1 文章目录 前言一、RR机器人创建description pkg创建demos pkg 二、创建controller相关创建example pkg 三、测试运行总结 前言
本系列文件主要有以下目标和内容#xff1a;
为系统、传感器和执行器创建 Har…【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_demosros2 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.txtcmake_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/rvizDESTINATION share/${PROJECT_NAME}/rrbot
)ament_package()# packages.xml?xml version1.0?
?xml-model hrefhttp://download.ros.org/schema/package_format3.xsd schematypenshttp://www.w3.org/2001/XMLSchema?
package format3nameros2_control_demo_description/nameversion0.0.0/versiondescriptionTODO: Package description/descriptionmaintainer emailhttps://blog.csdn.net/Bing_Leedev/maintainerlicenseTODO: License declaration/licensebuildtool_dependament_cmake/buildtool_dependexec_dependjoint_state_publisher_gui/exec_dependexec_dependrobot_state_publisher/exec_dependexec_dependrviz2/exec_depend
/package# rrbot.rvizPanels:- Class: rviz_common/DisplaysHelp Height: 87Name: DisplaysProperty Tree Widget:Expanded: ~Splitter Ratio: 0.5Tree Height: 1096- Class: rviz_common/SelectionName: Selection- Class: rviz_common/Tool PropertiesExpanded:- /2D Goal Pose1- /Publish Point1Name: Tool PropertiesSplitter Ratio: 0.5886790156364441- Class: rviz_common/ViewsExpanded:- /Current View1Name: ViewsSplitter Ratio: 0.5
Visualization Manager:Class: Displays:- Alpha: 0.5Cell Size: 1Class: rviz_default_plugins/GridColor: 160; 160; 164Enabled: trueLine Style:Line Width: 0.029999999329447746Value: LinesName: GridNormal Cell Count: 0Offset:X: 0Y: 0Z: 0Plane: XYPlane Cell Count: 10Reference Frame: Fixed FrameValue: true- Alpha: 1Class: rviz_default_plugins/RobotModelCollision Enabled: falseDescription File: Description Source: TopicDescription Topic:Depth: 5Durability Policy: VolatileHistory Policy: Keep LastReliability Policy: ReliableValue: /robot_descriptionEnabled: trueLinks:All Links Enabled: trueExpand Joint Details: falseExpand Link Details: falseExpand Tree: falseLink Tree Style: Links in Alphabetic Orderbase_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truecamera_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truecamera_link_optical:Alpha: 1Show Axes: falseShow Trail: falsehokuyo_link:Alpha: 1Show Axes: falseShow Trail: falseValue: truelink1:Alpha: 1Show Axes: falseShow Trail: falseValue: truelink2:Alpha: 1Show Axes: falseShow Trail: falseValue: truetool_link:Alpha: 1Show Axes: falseShow Trail: falseworld:Alpha: 1Show Axes: falseShow Trail: falseMass Properties:Inertia: falseMass: falseName: RobotModelTF Prefix: Update Interval: 0Value: trueVisual Enabled: trueEnabled: trueGlobal Options:Background Color: 48; 48; 48Fixed Frame: base_linkFrame Rate: 30Name: rootTools:- Class: rviz_default_plugins/InteractHide Inactive Objects: true- Class: rviz_default_plugins/MoveCamera- Class: rviz_default_plugins/Select- Class: rviz_default_plugins/FocusCamera- Class: rviz_default_plugins/MeasureLine color: 128; 128; 0- Class: rviz_default_plugins/SetInitialPoseCovariance x: 0.25Covariance y: 0.25Covariance yaw: 0.06853891909122467Topic:Depth: 5Durability Policy: VolatileHistory Policy: Keep LastReliability Policy: ReliableValue: /initialpose- Class: rviz_default_plugins/SetGoalTopic:Depth: 5Durability Policy: VolatileHistory Policy: Keep LastReliability Policy: ReliableValue: /goal_pose- Class: rviz_default_plugins/PublishPointSingle click: trueTopic:Depth: 5Durability Policy: VolatileHistory Policy: Keep LastReliability Policy: ReliableValue: /clicked_pointTransformation:Current:Class: rviz_default_plugins/TFValue: trueViews:Current:Class: rviz_default_plugins/OrbitDistance: 8.443930625915527Enable Stereo Rendering:Stereo Eye Separation: 0.05999999865889549Stereo Focal Distance: 1Swap Stereo Eyes: falseValue: falseFocal Point:X: 0.0044944556429982185Y: 1.0785865783691406Z: 2.4839563369750977Focal Shape Fixed Size: trueFocal Shape Size: 0.05000000074505806Invert Z Axis: falseName: Current ViewNear Clip Distance: 0.009999999776482582Pitch: 0.23039916157722473Target Frame: Fixed FrameValue: Orbit (rviz)Yaw: 5.150422096252441Saved: ~
Window Geometry:Displays:collapsed: falseHeight: 1379Hide Left Dock: falseHide Right Dock: falseSelection:collapsed: falseTool Properties:collapsed: falseViews:collapsed: falseWidth: 2560X: 0Y: 1470# rrbot_description.urdf.xacro?xml version1.0?
robot xmlns:xacrohttp://www.ros.org/wiki/xacroxacro:macro namerrbot paramsparent prefix *origin!-- Constants for robot dimensions --xacro:property namemass value1 / !-- arbitrary value for mass --xacro:property namewidth value0.1 / !-- Square dimensions (widthxwidth) of beams --xacro:property nameheight1 value2 / !-- Link 1 --xacro:property nameheight2 value1 / !-- Link 2 --xacro:property nameheight3 value1 / !-- Link 3 --xacro:property nameaxel_offset value0.05 / !-- Space btw top of beam and the each joint --joint name${prefix}base_joint typefixedxacro:insert_block nameorigin /parent link${parent}/child link${prefix}base_link //joint!-- Base Link --link name${prefix}base_linkcollisionorigin xyz0 0 ${height1/2} rpy0 0 0/geometrybox size${width} ${width} ${height1}//geometry/collisionvisualorigin xyz0 0 ${height1/2} rpy0 0 0/geometrybox size${width} ${width} ${height1}//geometrymaterial nameorange//visual/linkjoint name${prefix}joint1 typecontinuousparent link${prefix}base_link/child link${prefix}link1/origin xyz0 ${width} ${height1 - axel_offset} rpy0 0 0/axis xyz0 1 0/dynamics damping0.7//joint!-- Middle Link --link name${prefix}link1collisionorigin xyz0 0 ${height2/2 - axel_offset} rpy0 0 0/geometrybox size${width} ${width} ${height2}//geometry/collisionvisualorigin xyz0 0 ${height2/2 - axel_offset} rpy0 0 0/geometrybox size${width} ${width} ${height2}//geometrymaterial nameyellow//visual/linkjoint name${prefix}joint2 typecontinuousparent link${prefix}link1/child link${prefix}link2/origin xyz0 ${width} ${height2 - axel_offset*2} rpy0 0 0/axis xyz0 1 0/dynamics damping0.7//joint!-- Top Link --link name${prefix}link2collisionorigin xyz0 0 ${height3/2 - axel_offset} rpy0 0 0/geometrybox size${width} ${width} ${height3}//geometry/collisionvisualorigin xyz0 0 ${height3/2 - axel_offset} rpy0 0 0/geometrybox size${width} ${width} ${height3}//geometrymaterial nameorange//visual/linkjoint name${prefix}tool_joint typefixedorigin xyz0 0 1 rpy0 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 version1.0?
!--
Copied from ROS1 example -
https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/materials.xacro
--
robotmaterial nameblackcolor rgba0.0 0.0 0.0 1.0//materialmaterial namebluecolor rgba0.0 0.0 0.8 1.0//materialmaterial namegreencolor rgba0.0 0.8 0.0 1.0//materialmaterial namegreycolor rgba0.2 0.2 0.2 1.0//materialmaterial nameorangecolor rgba${255/255} ${108/255} ${10/255} 1.0//materialmaterial namebrowncolor rgba${222/255} ${207/255} ${195/255} 1.0//materialmaterial nameredcolor rgba0.8 0.0 0.0 1.0//materialmaterial nameyellowcolor rgba1.0 1.0 0.0 1.0//materialmaterial namewhitecolor rgba1.0 1.0 1.0 1.0//material/robot创建demos pkg
这个包用于引导编译所有相关依赖包按照下边格式填好即可。
cd ~/ros2_control_demosros2 pkg create --build-type ament_cmake ros2_control_demos# 文件结构
$ tree ros2_control_demos/
ros2_control_demos/
├── CMakeLists.txt
└── package.xml# CMakeLists.txtcmake_minimum_required(VERSION 3.8)
project(ros2_control_demos)# find dependencies
find_package(ament_cmake REQUIRED)ament_package()# package.xml?xml version1.0?
?xml-model hrefhttp://download.ros.org/schema/package_format3.xsd schematypenshttp://www.w3.org/2001/XMLSchema?
package format3nameros2_control_demos/nameversion0.0.0/versiondescriptionTODO: Package description/descriptionmaintainer emailhttps://blog.csdn.net/Bing_Leedev/maintainerlicenseTODO: License declaration/licensebuildtool_dependament_cmake/buildtool_dependexec_dependros2_control_demo_example_1/exec_dependexportbuild_typeament_cmake/build_type/export
/package二、创建controller相关
创建example pkg
cd ~/ros2_control_demosros2 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.hnamespace ros2_control_demo_example_1
{
class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface
{
public:RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware);ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIChardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo info) override;ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIChardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State previous_state) override;ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLICstd::vectorhardware_interface::StateInterface export_state_interfaces() override;ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLICstd::vectorhardware_interface::CommandInterface export_command_interfaces() override;ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIChardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State previous_state) override;ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIChardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State previous_state) override;ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIChardware_interface::return_type read(const rclcpp::Time time, const rclcpp::Duration period) override;ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIChardware_interface::return_type write(const rclcpp::Time time, const rclcpp::Duration period) override;private:// Parameters for the RRBot simulationdouble hw_start_sec_;double hw_stop_sec_;double hw_slowdown_;// Store the command for the simulated robotstd::vectordouble hw_commands_;std::vectordouble 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 packages 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”))) 是 GCCGNU 编译器集合中的一个特性它允许程序员控制特定部分的代码的可见性。在 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.hppnamespace 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_limitsdouble::quiet_NaN());hw_commands_.resize(info_.joints.size(), std::numeric_limitsdouble::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::vectorhardware_interface::StateInterface
RRBotSystemPositionOnlyHardware::export_state_interfaces()
{std::vectorhardware_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::vectorhardware_interface::CommandInterface
RRBotSystemPositionOnlyHardware::export_command_interfaces()
{std::vectorhardware_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){// 仿真发送命令到hardwareRCLCPP_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.hppPLUGINLIB_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 ↩︎
