机器人的ros_control硬件接口设计_ros创建自己的硬件接口

下面我们为一个六轴机械臂设计硬件节点。当然，你可以任意创建自己的机器人模型，原理都是一样的。其实现由MYROBOT_Hardware和MYROBOT_HardwareInterface两个类构成，在其中创建了JointStateInterface，提供关节状态信息的反馈，创建了PositionJointInterface，用于基于位置命令的控制器，包括单纯的位置命令控制器joint_group_position_controller，以及运动轨迹的控制器joint_trajectory_controller，joint_trajectory_controller便可以用于与 Moveit进行对接。

这里还只是先实现了ROS端的内容，也就是说并没有真正完成硬件的控制，但可以窥探ros_control的工作的机制。真正的硬件控制可在完成这一部分之后在进行添加。

基本代码

MYROBOT_Hardware

MYROBOT_Hardware单纯地定义了ros_control需要各个接口和变量。

class MYROBOT_Hardware : public hardware_interface::RobotHW
{
protected:
  hardware_interface::JointStateInterface       jnt_state_interface_;
  hardware_interface::PositionJointInterface    jnt_pos_interface_;
  hardware_interface::VelocityJointInterface    jnt_vel_interface_;
  hardware_interface::EffortJointInterface      jnt_eff_interface_;

  joint_limits_interface::PositionJointSaturationInterface  jnt_pos_saturation_interface_;
  joint_limits_interface::PositionJointSoftLimitsInterface  jnt_pos_limits_interface_;
  //... and joint saturation and limits for position & velocity

  int                       num_jnts_;
  std::vector<std::string>  jnt_names_;
  std::vector<int>          jnt_type_;
  std::vector<double>       jnt_cmd_;
  std::vector<double>       jnt_pos_;
  std::vector<double>       jnt_vel_;
  std::vector<double>       jnt_eff_;
  std::vector<double>       jnt_lower_limits_;
  std::vector<double>       jnt_upper_limits_;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

MYROBOT_HardwareInterface类定义

class MYROBOT_HardwareInterface: public MYROBOT_Hardware
{
  public:
    MYROBOT_HardwareInterface(ros::NodeHandle& nh);
    ~MYROBOT_HardwareInterface();
    void init();
    void update(const ros::TimerEvent& e);
    void read();
    void write(ros::Duration elapsed_time);
  protected:
    ros::NodeHandle nh_;  //
    ros::Timer timer_;  //定期更新的定时器
    double control_period_;  //控制周期
    ros::Duration elapsed_time_;
    boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

构造函数

MYROBOT_HardwareInterface::MYROBOT_HardwareInterface(ros::NodeHandle& nh) \
		: nh_(nh)
{
  //1. 调用init完成接口的注册
  init();
  //2. 创建
  controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
  nh_.param("/myrobot/hardware_interface/control_period", control_period_, 0.1);
  //3. 创建用于定期更新的定时器
  non_realtime_loop_ = nh_.createTimer(ros::Duration(control_period_), &MYROBOT_HardwareInterface::update, this);
}
1
2
3
4
5
6
7
8
9
10
11

第2步中从"/myrobot/hardware_interface/control_period"读取更新周期。关于硬件的配置都放在如下的hardware.yaml文件中，由launch文件进行加载。

hrt120:
  hardware_interface:
     control_period: 0.01 # second
     joints:
       - joint_1
       - joint_2
       - joint_3
       - joint_4
       - joint_5
       - joint_6
1
2
3
4
5
6
7
8
9
10

init()函数

void control_period_HardwareInterface::init()
{
  //1. 取得关节名
  nh_.getParam("/myrobot/hardware_interface/joints", joint_names_);
  if (joint_names_.size() == 0)
  {
    ROS_FATAL_STREAM_NAMED("init", "No joints found on '/myrobot/hardware_interface/joints'");
  }
  num_joints_ = joint_names_.size();

  //2. 按指定的关节数重新分配空间
  num_jnts_ = jnt_names_.size();
  jnt_cmd_.resize(num_jnts_);
  jnt_pos_.resize(num_jnts_);
  jnt_vel_.resize(num_jnts_);
  jnt_eff_.resize(num_jnts_);

  //3. 初始化各关节对应的各种接口
  for(int i= 0; i<num_jnts_; i++) {
    cout<<"\t"<<jnt_names_[i]<<endl;
    // 状态Handle
    JointStateHandle jointStateHandle(jnt_names_[i], &jnt_pos_[i], &jnt_vel_[i], &jnt_eff_[i]);
    jnt_state_interface_.registerHandle(jointStateHandle);
    // 位置Handle
    JointHandle jointPositionHandle(jointStateHandle, &jnt_cmd_[i]);
    jnt_pos_interface_.registerHandle(jointPositionHandle);
  }
  //4. 注册接口
  registerInterface(&jnt_state_interface_);
  registerInterface(&jnt_pos_interface_);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

在第1步中，从"/myrobot/hardware_interface/joints"参数中读取关节名称，该参数同样在hardware.yaml中。

update()函数

这个函数会根据构造函数中定义的定时器周期进行执行，处理

void MYROBOT_HardwareInterface::update(const ros::TimerEvent& e)
{
  //计算实际的时间间隔，因为定时不一定非常精确
  elapsed_time_ = ros::Duration(e.current_real - e.last_real);
  //读取硬件关节的状态
  read();
  controller_manager_->update(ros::Time::now(), elapsed_time_);
  //发送关节指令给硬件。
  write(elapsed_time_);
}
1
2
3
4
5
6
7
8
9
10

HRT_hardware_interface_node

int main(int argc, char** argv)
{
  ros::init(argc, argv, "hardware_interface");
  ros::CallbackQueue ros_queue;

  ros::NodeHandle nh;
  nh.setCallbackQueue(&ros_queue);
  MYROBOT_HardwareInterface myrobot(nh);

  ros::MultiThreadedSpinner spinner(0);
  spinner.spin(&ros_queue);

  return 0;

1
2
3
4
5
6
7
8
9
10
11
12
13
14

相关配置文件

参数文件

ros_controllers.yaml

joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - joint_1
    - joint_2
    - joint_3
    - joint_4
    - joint_5
    - joint_6
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      joint_1: {trajectory: 0.1, goal: 0.1}
      joint_2: {trajectory: 0.1, goal: 0.1}
      joint_3: {trajectory: 0.1, goal: 0.1}
      joint_4: {trajectory: 0.1, goal: 0.1}
      joint_5: {trajectory: 0.1, goal: 0.1}
      joint_6: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10
joint_group_position_controller:
  type: position_controllers/JointGroupPositionController
  joints:
    - joint_1
    - joint_2
    - joint_3
    - joint_4
    - joint_5
    - joint_6
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

启动文件

load_model.launch

<launch>
  <!-- The name of the parameter under which the URDF is loaded -->
  <arg name="robot_description" default="robot_description"/>

  <!-- Load universal robot description format (URDF) -->
  <param name="$(arg robot_description)" command="xacro  '$(find myrobot_description)/urdf/myrobot.xacro'"/>
</launch>
1
2
3
4
5
6
7

hardware.launch

<?xml version="1.0"?>
<launch>
  <!-- Load hardware configurations from YAML file to parameter server -->
  <rosparam file="$(find myrobt_hardware)/config/hardware.yaml" command="load"/>

  <!-- start myrobot hardware node -->
  <node name="myrobt_hardware" pkg="myrobt_hardware" type="myrobot_hardware_node" respawn="false" output="screen"/>
</launch>
1
2
3
4
5
6
7
8

ros_controllers.launch

<?xml version="1.0"?>
<launch>
  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find myrobt_hardware)/config/ros_controllers.yaml" command="load"/>

  <!-- Load & start joint_state_controller -->
  <node name="joint_state_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>

  <!-- Load & start arm_controller -->
  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>

  <!-- Load joint_group_position_controller -->
  <node name="controller_loader" pkg="controller_manager" type="controller_manager" args="load joint_group_position_controller" respawn="false" output="screen"/>

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

在控制器加载时会根据控制器的名称在参数服务器上找到提前加载好的对应名称项。根据控制器名称下面的type来选择相应的控制器。可以看到配置的两个控制器都是位置相关的控制，因此在硬件接口实现中只需要实现位置接口即可。
另外，上面的launch文件中分别使用controller_manager的"spawn"来加载和启动"arm_controller"和"joint_state_controller，而使用"load"仅仅加载joint_group_position_controller。这是因为arm_controller和joint_group_position_controller是使用的同样的资源的，即MYROBOT_hardware中的jnt_cmd_变量，所以就会有资源冲突，只能有一个处于运行状态。

基本测试

现在就来测试所编写硬件接口是否正常工作。

加载模型

首先加载机器人模型，新建一个终端，运行如下命令：

roslaunch myrobot_hardware load_model.launch
1

正常运行之后会将模型描述加载到’/robot_description’中。

运行硬件接口节点

新建一个终端，运行如下命令：

roslaunch myrobot_hardware hardware.launch
1

该节点会注册硬件接口，并创建一个controller_manager类的对象。controller_manager提供两个主要功能：管理将要加载的控制器，提供查询和切换控制器的ROS服务。

加载控制器

新建一个终端，运行如下命令：

roslaunch myrobot_hardware ros_controllers.launch
1

该launch文件会通过controller_manager加载两个控制器，并与硬件接口中提供的PositionJointInterface相关联。
加载的joint_state_controller会透过MYROBOT_HardwareInterface提供的JointStateInterface获取状态变量直并发布到"/joint_states"主题上

加载的arm_controller是JointTrajectoryController，这个控制器除了提供command主题之外，还创建一个Action Server，提供action相关的主题，可供Moveit使用。

简单的控制

joint_group_position_controller

首先将控制器切换到joint_group_position_controller：

rosservice call /controller_manager/switch_controller "start_controllers:
- 'joint_group_position_controller'
stop_controllers:
- 'arm_controller'
strictness: 2"
1
2
3
4
5

或者直接在ros_controllers.launch文件中直接调换"load"和"spawn",使得启动的控制器为joint_group_position_controller。

然后在MYROBOT_hardwareinterface的write函数中分别写入代码：

void MYROBOT_HardwareInterface::write(ros::Duration elapsed_time)
{
	cout<<"cmd:\t";
	for(int i=0; i<num_jnts_; i++ )
	{
		cout<<jnt_cmd_[i]<<"\t";
	}
	cout<<endl;
}
1
2
3
4
5
6
7
8
9

最后新开一个终端，运行如下命令：

rostopic pub /joint_group_position_controller/comma std_msgs/Float64MultiArray "layout:
  dim:
  - label: ''
    size: 6
    stride: 0
  data_offset: 0
data: [0.1,0.2,0.3,0.4,0.5,0.6]"
1
2
3
4
5
6
7

可以使用tab键得到内容提示并不全。然后我们就会在运行硬件接口节点的那个终端窗口中看到如下改变：

cmd:	0.1	0.2	0.3	0.4	0.5	0.6	
cmd:	0.1	0.2	0.3	0.4	0.5	0.6	
cmd:	0.1	0.2	0.3	0.4	0.5	0.6
...
1
2
3
4

这说明对于joint_group_position_controller来说，当给控制器下命令时，控制器确实是将命令传递给了MYROBOT_hardware硬件接口。另外开一个终端显示/joint_states的内容，发现位置、速度、力/力矩的值一直都为零：

rostopic echo /joint_states
...
position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
1
2
3
4
5

这是因为我们并没有写更新状态的代码，例如获取真实的硬件关节的位置并将之写入状态变量等等。为了说明确实可以通过/joint_states主题获取到关节状态，我们假设前面的位置命令被虚拟机器人立即完全执行了。因此在read函数中将命令变量值直接复制给位置状态变量：

void MYROBOT_HardwareInterface::read()
{
  for(int i=0; i<num_jnts_; i++ )
  {
    jnt_pos_[i] = jnt_cmd_[i];
  }
}
1
2
3
4
5
6
7

编译后再重新运行，查看/joint_states的位置内容便和给的cmd一样并随之变化了。

arm_controller

这是一个JointTrajectoryController类型的控制器，它所接收的命令是一组运动轨迹点，使用trajectory_msgs/JointTrajectory消息。由于数据变得复杂，因此再使用命令行的方式输入轨迹命令变得不那么可取，我们转而使用Moveit+RViz的方式提供轨迹命令。

首先将ROS控制器切换到arm_controller。

使用moveit_setup_assistant对机器人进行moveit相关配置，这里不做论述，得到的是myrobot_moveit_config包。

然后检查myrobot_moveit_config/config/ros_controllers.yaml中controller_list的内容，其需要与ROS下的arm_controller一致。其内容为：

controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
1
2
3
4
5
6
7
8
9
10
11
12

name必须是ROS下的arm_controller，因为arm_controller启动的ActionServer的主题形如"/arm_controller/follow_joint_trajectory/XXX"。

检查myrobot_moveit_config/launch/move_group.launch文件中fake_execution是否为ture，因为我们不是使用moveit中的fake_controller_manager，而是使用moveit中的moveit_simple_controller_manager与arm_controller进行通信，所以fake_execution要设置为false。

现在执行如下命令：

roslaunch myrobot_moveit_config move_group.launch
1

该启动文件会启动MoveIt的move_group节点。 接下来打开RViz：

roslaunch myrobot_moveit_config moveit_rviz.launch config:=true
1

在打开的Rviz界面中拖动机器人末端的Interactive Marker到某个位置，然后点击MotionPlanning面板中的Plan and Execute按钮进行轨迹规划和执行。执行时我们可以看到运行硬件接口节点的终端窗口中会不断地输出轨迹点的关节位置命令信息：

...
cmd:  -0.00511592   1.27753   -1.14339  -3.21831e-05  -0.13408  -0.00500986
cmd:  -0.00526443   1.27577   -1.14079  -3.20341e-05  -0.13492  -0.00515844
cmd:  -0.00541904   1.27394   -1.13808  -3.18789e-05  -0.135796 -0.00531311
cmd:  -0.00556922   1.27216   -1.13546  -3.17281e-05  -0.136646 -0.00546335
cmd:  -0.00573604   1.27019   -1.13254  -3.15607e-05  -0.13759  -0.00563025
cmd:  -0.00589038   1.26836   -1.12984  -3.14057e-05  -0.138464 -0.00578466
cmd:  -0.00606748   1.26626   -1.12674  -3.1228e-05   -0.139466 -0.00596183
cmd:  -0.00624911   1.26411   -1.12356  -3.10456e-05  -0.140494 -0.00614353
cmd:  -0.00643485   1.26191   -1.12031  -3.08592e-05  -0.141545 -0.00632936
cmd:  -0.00660525   1.2599    -1.11733  -3.06881e-05  -0.14251  -0.00649983
cmd:  -0.00679885   1.25761   -1.11394  -3.04938e-05  -0.143606 -0.00669351
cmd:  -0.00699674   1.25526   -1.11048  -3.02952e-05  -0.144726 -0.00689148
...
1
2
3
4
5
6
7
8
9
10
11
12
13
14

同时/joint_states也是立即返回关节状体，从而RViz中能够实时地显示运动的过程。