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

田彼南山

于 2024-08-14 09:58:10 发布

阅读量261

点赞数 5

文章标签： ros2_control

本文链接：https://blog.csdn.net/weixin_41652700/article/details/139954389

版权

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, PathJoinSubstitution

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


def 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给出urdf
    robot_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";

}  // namespace

namespace 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 joint
    if (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 code
  RCLCPP_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 values
  for (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));
		
		});


    //创建实时Publisher
    cmd_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 code
  RCLCPP_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 code

  RCLCPP_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)