ROS系统MoveIt玩转双臂机器人系列(三)–利用controller控制实际机器人

518
1
2020年8月10日 09时00分

注:本篇博文全部源码下载地址为:Git Repo

1. 下载到本地后解压到当前文件夹然后运行:catkin_make 编译。

2. 源码是在 Ubuntu14.04 + Indigo 环境下编写。

 

一、概述

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

 

ROS系统MoveIt玩转双臂机器人系列(三)–利用controller控制实际机器人插图

图1

 

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

二、改造默认生成的 rob_moveit_config 包

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

1. 创建 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_mover
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
      - r_shoulder_joint
      - r_bigarm_joint
      - r_rotatearm_joint
      - r_elbow_joint
      - r_wrist_joint

 - name: l_rob_mover
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
      - l_shoulder_joint
      - l_bigarm_joint
      - l_rotatearm_joint
      - l_elbow_joint
      - l_wrist_joint

 

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

2. 改写 rob_robot_moveit_controller_manager.launch.xml 文件

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

 

<launch>
    <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

    <!-- 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 文件的配置参数

<arg name=”fake_execution” value=”true”/>  改为  <arg name=”fake_execution” value=”false”/>

 

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

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

 

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

 

<launch>
  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" />
  <!-- Allow user to specify database location -->
  <arg name="db_path" default="$(find rob_moveit_config)/default_warehouse_mongo_db" />
  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />
  <arg name="use_gui" default="false" />
  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find rob_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>
  <!-- If needed, broadcast static tf for robot root --> 
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <!-- <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam> -->
    <rosparam param="/source_list">[/joint_states]</rosparam>
  </node>
  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find rob_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="false"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>
  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find rob_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>
  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(find rob_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  </include>
  <!-- start rob control -->
  <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 后的程序包结构如下,红色部分为新添加的包。

 

ROS系统MoveIt玩转双臂机器人系列(三)–利用controller控制实际机器人插图(1)

 

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

 

ROS系统MoveIt玩转双臂机器人系列(三)–利用controller控制实际机器人插图(2)

图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 MoveIT
    R_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 JointTrajectoryAction
void 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 add
  for(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 degree
      rs.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 msg
 ROS_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所示。

 

ROS系统MoveIt玩转双臂机器人系列(三)–利用controller控制实际机器人插图(3)

图3

 

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

 

ROS系统MoveIt玩转双臂机器人系列(三)–利用controller控制实际机器人插图(4)

图4

 

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

 

ROS系统MoveIt玩转双臂机器人系列(三)–利用controller控制实际机器人插图(5)

图5

 

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

 

<– 本篇完 –>

 

本文转载自博客园,原文链接:https://www.cnblogs.com/shawn0102/p/9021702.html

发表评论

后才能评论

评论列表(1条)

  • j4gm8_6218 2020年8月17日 下午9:45

    请问老师为什么在wiki上control_msgs里没有FollowJointTrajectoryGoalConstPtr的定义,还有action是ROS2特有的吗,你的书里也没有讲到action