ROS 系统 MoveIt 玩转双臂机器人系列（三）-- 利用 controller 控制实际机器人

2025-08-03
四川
本文字数：4675 字
阅读完需：约 15 分钟

一、概述

本系列博文的第一篇，我们完成了双臂机器人 Rob 的建模工作；第二篇博文则详细介绍 MoveIt 模块的配置工具 Setup Assistant Tool 的使用，并配置了一个简单的使用环境。具体在程序结构上如图 1 所示，我们已经完成了两处红色椭圆圈出的部分，即完成了 ROS Param Server （建模）的工作，同时利用系统工具配置了 move_group 节点。

图 1

但是，到目前为止我们仍然只是在模拟环境中（Rviz）看到我们的机器人做动作，仍然没有和现实中的机器人产生关联，如何利用这个框架控制现实中的机器人则是本博文需要解决的问题。具体到程序结构上如图 1 所示，我们要完成绿色椭圆圈出的两个部分的工作，即将 MoveIt 的运动消息发送给实际的机器人，同时利用发布机器人的关节信息，实现方法如下文所述。

二、改造默认生成的 rob_moveit_config 包

我首先是直接对上一篇博文中生成的 rob_moveit_config 包进行改造，使其满足我们的实际使用要求，具体过程如下：

创建 controllers.yaml 文件。

首先在 src/rob_moveit_pack/rob_moveit_config/config 文件夹下，新建一个文件 controllers.yaml，这个文件是机器人控制器的配置（定义）文件，我们创建它是用于代替在这个目录下的 fake_controllers.yaml 文件的，这个控制器用于直接和我们的机器人进行交互。

然后打开 controllers.yaml 文件然后填写以下内容：

#controller_manager_ns: controller_manager

controller_list:

name: r_rob_moveraction_ns: follow_joint_trajectorytype: FollowJointTrajectorydefault: truejoints:
r_shoulder_joint
r_bigarm_joint
r_rotatearm_joint
r_elbow_joint
r_wrist_joint
name: l_rob_moveraction_ns: follow_joint_trajectorytype: FollowJointTrajectorydefault: truejoints:
l_shoulder_joint
l_bigarm_joint
l_rotatearm_joint
l_elbow_joint
l_wrist_joint

这里我们定义了两个控制器，分别是控制右臂的 r_rob_mover 和控制左臂的 l_rob_mover ，并分别制定了两个控制器下面控制的关节名称。注意，控制器名字我们自己定义，其余部分均为 ROS 默认的需要的配置，关节名称需要和机器人模型文件（xacro）中的名称一致。

改写 rob_robot_moveit_controller_manager.launch.xml 文件。

在 src/rob_moveit_pack/rob_moveit_config/launch 文件加下，找到 rob_robot_moveit_controller_manager.launch.xml 文件，这个文件用于配置 MoveIt 的控制器，我们需要将 MoveIt 的控制器指向我们上一步创建的控制器，此文件内容改为如下：

<!-- load controller_list --><arg name="use_controller_manager" default="true" /><param name="use_controller_manager" value="$(arg use_controller_manager)" />
<!-- Load joint controller configurations from YAML file to parameter server --><rosparam file="$(find rob_moveit_config)/config/controllers.yaml"/>

复制代码

</launch>

3. 改写 demo.launch 文件。

具体来说修改一下几个地方：

<1> 改写 joint_state_publisher 节点启动参数

<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam> 改为 <rosparam param="/source_list">[/joint_states]</rosparam>；

具体话题 /joint_states 的由来详见下文。

<2> 改写 move_group.launch 文件的配置参数

<3> 添加 rob_control 节点的启动文件

具体 rob_control 节点的由来详见下一章节。

修改完成的 demo.launch 文件如下所示：

<launch><arg name="db" default="false" /><arg name="db_path" default=" $(f i n d r o b_{m} o v e i t_{c} o n f i g) / d e f a u l t_{w} a r e h o u s e_{m} o n g o_{d} b " / > <! - - B y d e f a u l t, w e a r e n o t i n d e b u g m o d e - - > < a r g n a m e = " d e b u g " d e f a u l t = " f a l s e " / > < a r g n a m e = " u s e_{g} u i " d e f a u l t = " f a l s e " / > <! - - L o a d t h e U R D F, S R D F a n d o t h e r . y a m l c o n f i g u r a t i o n f i l e s o n t h e p a r a m s e r v e r - - > <arg name="load_robot_description" value="true"/></include><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"><param name="/use_gui" value=" $(a r g u s e_{g} u i) " / > <! - - < r o s p a r a m p a r a m = " / s o u r c e_{l} i s t " > [/ m o v e_{g} r o u p / f a k e_{c} o n t r o l l e r_{j} o i n t_{s} t a t e s] < / r o s p a r a m > - - > < r o s p a r a m p a r a m = " / s o u r c e_{l} i s t " > [/ j o i n t_{s} t a t e s] < / r o s p a r a m > < / n o d e > <! - - G i v e n t h e p u b l i s h e d j o i n t s t a t e s, p u b l i s h t f f o r t h e r o b o t l i n k s - - > < n o d e n a m e = " r o b o t_{s} t a t e_{p} u b l i s h e r " p k g = " r o b o t_{s} t a t e_{p} u b l i s h e r " t y p e = " r o b o t_{s} t a t e_{p} u b l i s h e r " r e s p a w n = " t r u e " o u t p u t = " s c r e e n " / > <! - - R u n t h e m a i n M o v e I t e x e c u t a b l e w i t h o u t t r a j e c t o r y e x e c u t i o n (w e d o n o t h a v e c o n t r o l l e r s c o n f i g u r e d b y d e f a u l t) - - > <arg name="allow_trajectory_execution" value="true"/><arg name="fake_execution" value="false"/><arg name="info" value="true"/><arg name="debug" value=" $(a r g d e b u g) " / > <! - - R u n R v i z a n d l o a d t h e d e f a u l t c o n f i g t o s e e t h e s t a t e o f t h e m o v e_{g} r o u p n o d e - - > <arg name="config" value="true"/><arg name="debug" value=" $(a r g d e b u g) " / > <! - - I f d a t a b a s e l o a d i n g w a s e n a b l e d, s t a r t m o n g o d b a s w e l l - - > < a r g n a m e = " m o v e i t_{w} a r e h o u s e_{d} a t a b a s e_{p} a t h " v a l u e = "$ (arg db_path)"/></include><include file="$(find rob_control)/launch/rob_control.launch" >launch-prefix="xterm -e"</include></launch>

三、添加 rob_control 包

至此，我们需要添加一个 Package 用于创建一个新的节点（Node），这个节点我们起名为 rob_control , 他的作用是：1. 处理 MoveIt 产生的运动消息，并发送给实际的机器人；2. 接收机器人传感器产生的实际运动参数，发布机器人的各个关节的关节状态。添加完新的 package 后的程序包结构如下，红色部分为新添加的包。

src　　├── CMakeLists.txt　　└── rob_moveit_pack

├── rob_description

├── rob_moveit_config

├── rob_control　　　　│ ├── CMakeLists.txt　　　　│ ├── include　　　　│ │ └── rob_control　　　　│ │ 　　├── rob_comm.h　　　　│ │ 　　└── rob_control.h　　　　│ ├── launch　　　　│ │ 　└── rob_control.launch　　　　│ ├── package.xml　　　　│ └── src　　　　│ 　　├── rob_comm.cpp　　　　│ 　　└── rob_control.cpp

└── rob_moveit_pack　　　　　　├── CMakeLists.txt　　　　　　└── package.xml

这个包的程序结构如图 2 所示，接收 Move_Group 模块产生的机器人运动规划消息，然后通过一定的通信手段（如 CAN 通信、串口通信、TCP 等）发送真正的给机器人，同时接收机器人的关节信息（机器人各个关节传感器测得的实际关节数据），然后将这个数据发送发布到话题 /joint_states 上，这就是为什么第二部分的第 3 小节中讲到修改 demo.launch 文件中 joint_state_publisher 节点启动参数时订阅话题 /joint_states，只有这样才能实时更新机器人的状态，避免每次新的运动都从零状态出发。

图 2

我们知道，ROS 系统提供了一种具有抢占功能的 CS（Client - Server）节点（node）通信方式，就是 Actionlib，这里 MoveIt 发布机器人运动消息序列就是采用这种通信方式（FollowJointTrajectoryAction 接口），以便机器人可以随时更新状态并覆盖掉未执行的老的状态。这个接口的使用方法在 rob_control.cpp 源码中有详细的使用方法，这里只做简单说明。

<1> 首先做一个定义，方便后续使用

typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> R_TrajectoryServer;typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> L_TrajectoryServer;

<2> 然后对其进行定义和初始化

//Start the ActionServer for JointTrajectoryActions from MoveITR_TrajectoryServer r_tserver(n2, "r_rob_mover/follow_joint_trajectory", boost::bind(&r_executeTrajectory, _1, &r_tserver), false);ROS_INFO("TrajectoryActionServer: Starting");r_tserver.start();

L_TrajectoryServer l_tserver(n2, "l_rob_mover/follow_joint_trajectory", boost::bind(&l_executeTrajectory, _1, &l_tserver), false);  ROS_INFO("TrajectoryActionServer: Starting");  l_tserver.start();

复制代码

<3> 回调函数编写（以右臂为例）

// Processing and JointTrajectoryActionvoid r_executeTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& r_goal, R_TrajectoryServer* r_as){robotState rs;float lastDuration = 0.0;int nrOfPoints = r_goal->trajectory.points.size(); // Number of points to addfor(int i=0; i<nrOfPoints; i++){rs.j[0] = r_goal->trajectory.points[i].positions[3]*rad2deg; // ros values come in rad, internally we work in degreers.j[1] = r_goal->trajectory.points[i].positions[0]*rad2deg;rs.j[2] = r_goal->trajectory.points[i].positions[2]*rad2deg;rs.j[3] = r_goal->trajectory.points[i].positions[1]*rad2deg;rs.j[4] = r_goal->trajectory.points[i].positions[4]*rad2deg;

  float dtmp = r_goal->trajectory.points[i].time_from_start.toSec();  rs.duration = dtmp - lastDuration;// time_from_start is adding up, these values are only for the single motion  lastDuration = dtmp;  r_targetPointList.push_back(rs);

复制代码

}r_as->setSucceeded();//debug msgROS_INFO("right arm recv: %f %f %f %f %f ,duration: %f", rs.j[0],rs.j[1],rs.j[2],rs.j[3],rs.j[4],rs.duration);}

这里 rob_control 包给出了一个比较通用的程序框架，大家只需稍作修改就可以应在自己的机器人项目中了。

四、效果检验

编译完成后，在 workspace 目录下，运行 source devel/setup.bash 配置环境后直接运行：

roslaunch rob_moveit_config demo.launch

我们在 Rviz 环境中 MotionPlaning 窗口的 Planing 选项下点击 Updata 随机生成一个机器人姿态，然后分别点击 Plan 和 Execute 按钮，就完成了一次动作的运动规划，如图 3 所示。

图 3

这个时候运动规划传送的机器人关节运动序列就通过 rob_control 节点发送给机器人了，我们可以看到部分发送的 debug log 如图 4 所示。

图 4

整个程序各个节点以及话题之间的链接关系如图 5 所示，我们可以看到话题 /joint_states 的存在，我们就是通过它实时发布机器人的状态的，进而 tf 才能正常运转。

图 5

至此我们完成了 MoveIt 核心程序的配置以及和我们机器人实现了通信链接，本文给出了一个比较通用的程序框架，后面我们进一步介绍 MoveIt 的用户接口的使用（User Interface ）和双臂机器人的运动学相关知识。

发布于: 刚刚阅读数: 4

原文链接:【http://xie.infoq.cn/article/c0157002332af55ed54a604bf】。文章转载请联系作者。

芯动大师

关注

凡事预则立，不预则废！ 2022-06-01 加入

某公司芯片AE工程师，嵌入式开发工程师，InfoQ签约作者，阿里云专家博主，华为云·云享专家，51CTO专家博主，腾讯云社区优秀共创官。

发布

暂无评论

创作场景

ROS 系统 MoveIt 玩转双臂机器人系列（三）-- 利用 controller 控制实际机器人

芯动大师

评论