二自由度机械臂软件系统（三）ros2_control硬件底层插件

• ros2_control实现了两个功能，一个是控制算法插件即控制的实现，另一个是底层插件即跟硬件通信的功能。

参考资料：https://zhuanlan.zhihu.com/p/682574842

1、创建功能包

ros2 pkg create --build-type ament_cmake robot_control_test

• 在src中创建功能包并编译

2、创建launch文件

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitutionfrom launch_ros.actions import Node
from launch_ros.substitutions import FindPackageSharedef generate_launch_description():# Declare arguments 1、声明参数declared_arguments = []declared_arguments.append(DeclareLaunchArgument("description_file",default_value="robottest.urdf.xacro",    #这个名字是跟description中的xacro文件有关的description="URDF/XACRO description file with the robot.带有机器人的URDF/XACRO描述文件。",))declared_arguments.append(DeclareLaunchArgument("gui",default_value="true",description="Start Rviz2 and Joint State Publisher gui automatically \with this launch file.使用此启动文件自动启动Rviz2和Joint State Publisher gui。",))declared_arguments.append(DeclareLaunchArgument("prefix",default_value='""',description="Prefix of the joint names, useful for \multi-robot setup. If changed than also joint names in the controllers' configuration \have to be updated.\关节名称的前缀，用于多机器人设置。如果改变了，那么控制器配置中的联合名称也必须更新",))# Initialize Arguments  2、初始化参数description_file = LaunchConfiguration("description_file")gui = LaunchConfiguration("gui")prefix = LaunchConfiguration("prefix")# Get URDF via xacro   3、通过xacro给出urdfrobot_description_content = Command([PathJoinSubstitution([FindExecutable(name="xacro")])," ",PathJoinSubstitution([FindPackageShare("robot_test_control"), "urdf", description_file]),])robot_description = {"robot_description": robot_description_content}# rviz_config_file = PathJoinSubstitution(#     [FindPackageShare("motor_test_control"), "rviz", "motorbot_view.rviz"]# )########################################################################################joint_state_publisher_node = Node(package="joint_state_publisher_gui",executable="joint_state_publisher_gui",condition=IfCondition(gui),)#发布关节数据信息robot_state_publisher_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),)#rviz节点nodes = [joint_state_publisher_node,robot_state_publisher_node,rviz_node,]return LaunchDescription(declared_arguments + nodes)

• 启动文件分成了两部分，参数的存储和节点生成。前面都是在准备所需的参数，后面设置节点，最终利用LaunchDescription生成了节点。

节点名	作用
rviz_node	运行rviz
joint_state_publisher_node	用一个ui获得控制命令，并发布到状态节点
robot_state_publisher_node	主要用于发布机器人的urdf模型数据

3、机器人的xacro文件

（1）robottest.urdf.xacro

<?xml version="1.0"?>
<!-- Basic differential drive mobile base -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robotdrive_robot"><!-- 导入基本的几何形状 --><xacro:include filename="$(find robot_control_test)/urdf/robottest_description.urdf.xacro" /><!-- 导入 ros2_control 描述部分 --><xacro:include filename="$(find robot_control_test)/urdf/robottest.ros2_control.xacro" /></robot>

• 分别导入模型和控制器

（2）robottest.ros2_control.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"><ros2_control name="test_control123" type="system"><hardware><plugin>robot_control_test/RobotTestSystemHardware</plugin></hardware><joint name="link1_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint><joint name="link2_joint"><command_interface name="position"/><state_interface name="position"/><state_interface name="velocity"/></joint></ros2_control></robot>

• 这是导入控制器的代码，注意在这里引入了底层控制器的插件。这个插件就是之后要写代码编译生成的动态库

4、底层硬件控制器代码

#include "motor_test_system.hpp"#include <chrono>
#include <cmath>
#include <cstddef>
#include <limits>
#include <memory>
#include <vector>#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include "utils_can.h"utils_can motor1(1);
utils_can motor2(2);namespace
{
constexpr auto DEFAULT_COMMAND_TOPIC = "motor_test_cmd";
constexpr auto DEFAULT_STATE_TOPIC = "motor_test_state";}  // namespacenamespace motor_test_control
{
hardware_interface::CallbackReturn MotorTestSystemHardware::on_init(const hardware_interface::HardwareInfo & info)
{ std::cout << "初始化" << std::endl;//1、打开can盒int state = 0;state = VCI_OpenDevice(VCI_USBCAN2A,0,0);if(state==1)//打开设备{std::cout << "open deivce success!\n" <<std::endl;//打开设备成功}else if(state==-1){std::cout << "no can device!\n" <<std::endl;}else{printf(">>open deivce fail!\n");//这个就算成功打开，还是为0}//2、初始化两路can口//2.1 can1初始化VCI_INIT_CONFIG config_can;config_can.AccCode=0;config_can.AccMask=0xFFFFFFFF;config_can.Filter=1;//接收所有帧config_can.Mode=0;//正常模式config_can.Timing0=0x00;/*波特率1000 Kbps*/config_can.Timing1=0x14;std::cout <<"1000" <<std::endl;if(VCI_InitCAN(VCI_USBCAN2,0,0,&config_can)!=1){std::cout << ">>Init CAN fail" <<std::endl;VCI_CloseDevice(VCI_USBCAN2,0);}else{std::cout << ">>Init CAN success" <<std::endl;}if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1){VCI_CloseDevice(VCI_USBCAN2,0);}else{std::cout << ">>start CAN success" <<std::endl;}motor1.CanInit();motor2.CanInit();if (hardware_interface::SystemInterface::on_init(info) !=hardware_interface::CallbackReturn::SUCCESS){return hardware_interface::CallbackReturn::ERROR;}if (info_.joints.size() != 2){RCLCPP_FATAL(rclcpp::get_logger("MotorTestSystemHardware"),"The number of crawlers is set not correactly,please check your urdf. 2 is expected,but now it is %zu.", info_.joints.size());return hardware_interface::CallbackReturn::ERROR;}else{RCLCPP_INFO(rclcpp::get_logger("MotorTestSystemHardware"), "Found 2 crawlers sucessfully!");}hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());hw_velocities_.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){// DiffBotSystem has exactly two states and one command interface on each jointif (joint.command_interfaces.size() != 1){RCLCPP_FATAL(rclcpp::get_logger("MotorTestSystemHardware"),"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("MotorTestSystemHardware"),"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() != 2){RCLCPP_FATAL(rclcpp::get_logger("MotorTestSystemHardware"),"Joint '%s' has %zu state interface. 2 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("MotorTestSystemHardware"),"Joint '%s' have '%s' as first 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;}if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY){RCLCPP_FATAL(rclcpp::get_logger("MotorTestSystemHardware"),"Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);return hardware_interface::CallbackReturn::ERROR;}}return hardware_interface::CallbackReturn::SUCCESS;
}std::vector<hardware_interface::StateInterface> MotorTestSystemHardware::export_state_interfaces()
{std::vector<hardware_interface::StateInterface> state_interfaces;for (auto i = 0u; i < info_.joints.size(); i++){state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i]));state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i]));}return state_interfaces;
}std::vector<hardware_interface::CommandInterface> MotorTestSystemHardware::export_command_interfaces()
{std::vector<hardware_interface::CommandInterface> command_interfaces;for (auto i = 0u; 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 MotorTestSystemHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
{std::cout << "激活" << std::endl;// BEGIN: This part here is for exemplary purposes - Please do not copy to your production codeRCLCPP_INFO(rclcpp::get_logger("MotorTestSystemHardware"), "Activating ...please wait...");// END: This part here is for exemplary purposes - Please do not copy to your production code// set some default valuesfor (auto i = 0u; i < hw_positions_.size(); i++){if (std::isnan(hw_positions_[i])){hw_positions_[i] = 0;hw_velocities_[i] = 0;hw_commands_[i] = 0;}}subscriber_is_active_ = true;this->node_ =  std::make_shared<rclcpp::Node>("hardware_node");std_msgs::msg::Float64MultiArray empty_int16array;for(std::size_t i = 0; i < hw_positions_.size(); i++){empty_int16array.data.push_back(0.0);}received_fb_msg_ptr_.set(std::make_shared<std_msgs::msg::Float64MultiArray>(empty_int16array));fb_subscriber_ =this->node_->create_subscription<std_msgs::msg::Float64MultiArray>(DEFAULT_STATE_TOPIC, rclcpp::SystemDefaultsQoS(),[this](const std::shared_ptr<std_msgs::msg::Float64MultiArray> msg) -> void{if (!subscriber_is_active_){RCLCPP_WARN(this->node_->get_logger(), "Can't accept new commands. subscriber is inactive");return;}received_fb_msg_ptr_.set(std::move(msg));});//创建实时Publishercmd_publisher = this->node_->create_publisher<std_msgs::msg::Float64MultiArray>(DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS());realtime_cmd_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>(cmd_publisher);RCLCPP_INFO(rclcpp::get_logger("MotorTestSystemHardware"), "Successfully activated!");return hardware_interface::CallbackReturn::SUCCESS;
}hardware_interface::CallbackReturn MotorTestSystemHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
{std::cout << "反激活" << std::endl;// BEGIN: This part here is for exemplary purposes - Please do not copy to your production codeRCLCPP_INFO(rclcpp::get_logger("MotorTestSystemHardware"), "Deactivating ...please wait...");subscriber_is_active_ = false;fb_subscriber_.reset();received_fb_msg_ptr_.set(nullptr);// END: This part here is for exemplary purposes - Please do not copy to your production codeRCLCPP_INFO(rclcpp::get_logger("MotorTestSystemHardware"), "Successfully deactivated!");return hardware_interface::CallbackReturn::SUCCESS;
}hardware_interface::return_type motor_test_control::MotorTestSystemHardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{std::cout << "读" << std::endl;std::shared_ptr<std_msgs::msg::Float64MultiArray> fb_msg;received_fb_msg_ptr_.get(fb_msg);rclcpp::spin_some(this->node_);for (std::size_t i = 0; i < hw_positions_.size(); i++){// Update the joint status: this is a revolute joint without any limit.double pos_ = 0;int id_ = 0;motor1.receive_callback(pos_,id_);if(id_==1){std::cout << "id==1" << std::endl;
hw_positions_[0] = pos_;}else if(id_==2){std::cout << "id==2" << std::endl;
hw_positions_[1] = pos_;}else{std::cout << "else:" << id_<<std::endl;}if(i < hw_velocities_.size()){hw_velocities_[i] = fb_msg->data[i];// hw_positions_[i] = fb_msg->data[i];//+= period.seconds() * hw_velocities_[i];std::cout << "读:" << hw_positions_[i]<<std::endl;}}return hardware_interface::return_type::OK;
}hardware_interface::return_type motor_test_control::MotorTestSystemHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{std::cout << "写" << std::endl;if(realtime_cmd_publisher_->trylock()){auto & cmd_msg = realtime_cmd_publisher_->msg_;cmd_msg.data.resize(hw_commands_.size());for (auto i = 0u; i < hw_commands_.size(); i++){cmd_msg.data[i] = hw_commands_[i];std::cout << "写:" <<cmd_msg.data[i] <<std::endl;}motor1.send_callback(cmd_msg.data[0]/3.14*180.0);motor2.send_callback(cmd_msg.data[1]/3.14*180.0);realtime_cmd_publisher_->unlockAndPublish();}return hardware_interface::return_type::OK;
}}  // namespace diff_test_control#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(motor_test_control::MotorTestSystemHardware, hardware_interface::SystemInterface)