ROS-moveit！探索(4)webots和moveit联合控制[2]

开源代码：volcano_moveit webots版本：2021a ros版本：melodic

前言

在探索3中，笔者介绍了在webots中使用<extern>外部控制器和moveit联合控制的案例。这次笔者介绍如何在webots中使用ROS控制器和moveit联合控制。

1.通过探索3了解基本构成

moveit！的关系图如下所示：
ROS-moveit！探索(4)webots和moveit联合控制[2] 我们可以使用探索3中的例程来分析一下，我们需要处理什么数据。

打开webots，打开探索3中创建的世界
打开以下launch文件，设置所必须的ROS参数并且连接Webots中的UR机械臂
```
$ roslaunch ur_e_webots ur5e.launch
```
连接成功后，查看当前系统中的话题列表，如下所示。

/follow_joint_trajectory开头的topic是Moveit！最终规划发布的action(_第二节介绍_)消息，由机器人控制器端接受该消息后控制机器人完成运动。
/joint_states由机器人控制器端发布，主要发布机器人的当前状态信息，rostopic echo一下。由此也是可以看出：
1. 我们需要发布/joint_states话题
2. 需要发布 /follow_joint_trajectoryaction消息

2. action消息是什么

2.1 什么是action

ROS中有一个actinlib的功能包集，实现了action的通讯机制。那么什么是action呢？action也是一种类似于service的问答通讯机制，不一样的地方是action还带有一个反馈机制，可以不断反馈任务的实施进度，而且可以在任务实施过程中，中止运行。我们使用action来发布一个机器人的运动目标，机器人接到这个action后，就开始运动，在运动过程中不断反馈当前的运动状态，在运动过程中我们可以取消运动，让机器人停止，如果机器人完成了运动目标，那么action会返回任务完成的标志。

2.2 action的工作机制

action采用了服务器端/客户端（client and server）的工作模式，如下图所示：
ROS-moveit！探索(4)webots和moveit联合控制[2] client和server之间通过actionlib定义的“action protocol”进行通讯。这种通讯协议是基于ROS的消息机制实现的，为用户提供了client和server的接口，接口如下图所示：
在上边的action的接口框图上，我们可以看到，client向server端发布任务目标以及在必要的时候取消任务，server会向client发布当前的状态、实时的反馈和最终的任务结果。

goal：任务目标
cancel：请求取消任务
status：通知client当前的状态
feedback：周期反馈任务运行的监控数据

result：向client发送任务的执行结果，这个topic只会发布一次。想了解更多关于action的知识？古月居传送门

3. 逐步实现过程

3.1 新建功能包

至此，我们需要创建一个功能包用于创建一个新的Node，功能包名为volcano_moveit，他的作用是：

接受机器人产生的传感器数据，并且发布各个关节的关节状态话题/joint_states。
处理MoveIt产生的运动信息，发送给机器人。功能包结构如下： volcano_moveit
- launch
  - ur_webots.launch ->webots启动文件，前期配置
- msg ->和ROS联合webots实战案例(一)安装配置webots一样操作，将srv、msg文件复制进来
- src
  - robot_state_publisher.cpp ->发布/joint_states
  - trajectory_follower.cpp ->处理action信息
- srv
- world ->webots地图
- worlds ->webots自动产生的文件夹
- CMakeLists.txt

package.xml

3.2 配置文件

package.xml(笔者只展示因为moveit要添加的，其他不再赘述):

<!-- MoveIt -->
<exec_depend>controller_manager</exec_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>actionlib</build_depend>
<exec_depend>actionlib</exec_depend> 
<exec_depend>moveit_msgs</exec_depend>

CMakeLists.txt(笔者只展示因为moveit要编辑的，其他不再赘述):

``` cmake
find_package(catkin REQUIRED COMPONENTS
actionlib
moveit_msgs
)
catkin_package(
CATKIN_DEPENDS
actionlib
moveit_msgs
)
add_executable(robot_state_publisher src/robot_state_publisher.cpp)
add_dependencies(robot_state_publisher webots_ros_generate_messages_cpp)
target_link_libraries(robot_state_publisher ${catkin_LIBRARIES})
add_executable(trajectory_follower src/trajectory_follower.cpp)
add_dependencies(trajectory_follower webots_ros_generate_messages_cpp)
target_link_libraries(trajectory_follower ${catkin_LIBRARIES})

### 3.3 配置launch文件
``` xml
<?xml version="1.0"?>
<launch>

  <param name="/use_sim_time" value="true" />
  <!-- 启动 Webots -->
  <arg name="no_gui" default="false," doc="Start Webots with minimal GUI"/>
  <include file="$(find webots_ros)/launch/webots.launch">
    <arg name="mode" value="realtime"/>
    <arg name="no_gui" value="$(arg no_gui)"/>
    <arg name="world" value="$(find volcano_moveit)/world/moveit.wbt"/>
  </include>

  <!-- 加载模型 -->
  <include file="$(find ur_e_description)/launch/ur5e_upload.launch">
    <arg name="limited" value="false"/>
  </include>

  <!-- 启动控制器指向arm_controller_ur5e.yaml -->
  <rosparam file="$(find ur_e_gazebo)/controller/arm_controller_ur5e.yaml" command="load"/>
  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>

  <!-- load other controllers -->
  <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="load joint_group_position_controller" />
</launch>

3.4 实现/joint_states发布（robot_state_publisher.cpp）

先看一下webots发布了什么service,可以从下面的图看到ur5e发布了位置传感器的使能服务。

由于发布/joint_states需要获取position信息，需要使能电机的位置传感器，在代码中可以这么写(代码片段)：

#include <sensor_msgs/JointState.h>
static const char *armNames[NMOTORS] = {"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"};
int main(int argc, char **argv) 
{
 // 使能所有电机的位置传感器 
 ros::ServiceClient position_sensor_client;
 webots_ros::set_int position_sensor_srv;
 for (int i = 0; i < NMOTORS; i++)
 {
     position_sensor_client = n->serviceClient<webots_ros::set_int>(string("/ur5e/")+string(armNames[i])+string("_sensor/enable"));
     position_sensor_srv.request.value = TIME_STEP;
     if (position_sensor_client.call(position_sensor_srv) && position_sensor_srv.response.success)     
         ROS_INFO("Enabled %s successful.", armNames[i]);   
     else     
         ROS_ERROR("Failed to enabled %s.", armNames[i]);
 }
}

使能完service后，webots会发布各个电机的position话题，如下所示：

代码获取所有数据，实现代码如下(代码片段)。

``` c++
vector armpositions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
int main(int argc, char **argv)
{
ros::Subscriber sub_elbow_joint_sensor;
sub_elbow_joint_sensor = n->subscribe(“/ur5e/“+string(armNames[0])+”_sensor/value”,1,elbow_joint_sensorcallback);
ros::Subscriber sub_shoulder_lift_joint_sensor;
sub_shoulder_lift_joint_sensor = n->subscribe(“/ur5e/“+string(armNames[1])+”_sensor/value”,1,shoulder_lift_joint_sensor_callback);
ros::Subscriber sub_shoulder_pan_joint_sensor;
sub_shoulder_pan_joint_sensor = n->subscribe(“/ur5e/“+string(armNames[2])+”_sensor/value”,1,shoulder_pan_joint_sensor_callback);
ros::Subscriber sub_wrist_1_joint_sensor;
sub_wrist_1_joint_sensor = n->subscribe(“/ur5e/“+string(armNames[3])+”_sensor/value”,1,wrist_1_joint_sensorcallback);
ros::Subscriber sub_wrist_2_joint_sensor;
sub_wrist_2_joint_sensor = n->subscribe(“/ur5e/“+string(armNames[4])+”_sensor/value”,1,wrist_2_joint_sensorcallback);
ros::Subscriber sub_wrist_3_joint_sensor;
sub_wrist_3_joint_sensor = n->subscribe(“/ur5e/“+string(armNames[5])+”_sensor/value”,1,wrist_3_joint_sensorcallback);

ros::Publisher pub_Joint_State_Publisher;
pub_Joint_State_Publisher = n->advertise(“/joint_states”,1);
}

void elbow_joint_sensorcallback(const webots_ros::Float64Stamped::ConstPtr &value){
armpositions[0] = value->data;
}

void shoulder_lift_joint_sensor_callback(const webots_ros::Float64Stamped::ConstPtr &value){
armpositions[1] = value->data;
}

void shoulder_pan_joint_sensor_callback(const webots_ros::Float64Stamped::ConstPtr &value){
armpositions[2] = value->data;
}

void wrist_1_joint_sensorcallback(const webots_ros::Float64Stamped::ConstPtr &value){
armpositions[3] = value->data;
}

void wrist_2_joint_sensorcallback(const webots_ros::Float64Stamped::ConstPtr &value){
armpositions[4] = value->data;
}

void wrist_3_joint_sensorcallback(const webots_ros::Float64Stamped::ConstPtr &value){
armpositions[5] = value->data;
}

5. 获取完所有数据就需要将所有数据整合起来一起publish出去，实现代码如下(代码片段)：
``` c++
void Joint_State_Publish(ros::Publisher pub){
    sensor_msgs::JointState joint_state;
    joint_state.header.stamp = ros::Time::now();
    joint_state.header.frame_id = "sim";
    for (int i = 0; i < NMOTORS; i++)
    {
        joint_state.name.push_back(armNames[i]);
        joint_state.position.push_back(armpositions[i]);
        joint_state.velocity.push_back(0.0);
        joint_state.effort.push_back(0.0);
    }
    pub.publish(joint_state);
}

01.发布效果
ROS-moveit！探索(4)webots和moveit联合控制[2]

3.5 实现/follow_joint_trajectory action消息发布（trajectory_follower.cpp）

由于这写数据是由action消息发布的。首先要导入必备头文件：

#include <actionlib/server/simple_action_server.h>  //action服务
#include <control_msgs/FollowJointTrajectoryAction.h>   // 信息类型

在webots中使用position控制比使用velocity控制更加精准，但是需要设置最大速度小一点，保证机械臂看起来运行流畅（也可以通过PID调节速度、流畅度），设置电机最大速度代码如下（代码片段）：

//初始化电机
for (int i = 0; i < NMOTORS; ++i) { 
 set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/ur5e/") + string(armNames[i]) + string("/set_velocity"));   
 set_velocity_srv.request.value = 0.21;   
 if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
     ROS_INFO("Max Velocity set to 0.21 for motor %s.", armNames[i]);   
 else     
     ROS_ERROR("Failed to call service set_velocity on motor %s.", armNames[i]);
}

我们知道，ROS系统提供了一种具有抢占功能的CS（Client – Server）节点（node）通信方式，就是Actionlib，这里MoveIt 发布机器人运动消息序列就是采用这种通信方式（FollowJointTrajectoryAction 接口），以便机器人可以随时更新状态并覆盖掉未执行的老的状态。

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

typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;

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

Server server(*n, "follow_joint_trajectory", boost::bind(&on_goal,_1,&server), false);
ROS_INFO("TrajectoryActionServer: Starting");
server.start();

回调函数编写

void on_goal(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as){
  int PointsSize = goal->trajectory.points.size();
  ROS_INFO("data size %d",PointsSize);
  for (int i = 0; i < PointsSize; i++)
  {
      armpositions = goal->trajectory.points[i].positions;
      for (int j = 0; j < NMOTORS; j++)
      {
          set_position_client = n->serviceClient<webots_ros::set_float>(string("/ur5e/") + string(armNames[j]) + string("/set_position"));   
          set_position_srv.request.value = armpositions[j];
          if (set_position_client.call(set_position_srv) && set_position_srv.response.success)     
              ROS_INFO("Position set to %f for motor %s.", armpositions[j], armNames[j]);   
          else     
              ROS_ERROR("Failed to call service set_position on motor %s.", armNames[j]);
      }
  }
  as->setSucceeded();
}

4. 测试

打开webots仿真环境

$ roslaunch volcano_moveit ur_webots.launch

将控制器改为ROS ROS-moveit！探索(4)webots和moveit联合控制[2]

发布/joint_states

$ rosrun volcano_moveit robot_state_publisher

发布action

$ rosrun volcano_moveit trajectory_follower

使用universal_robot功能包下的launch文件启动Moveit！

$ roslaunch ur5_e_moveit_config ur5_e_moveit_planning_execution.launch

使用universal_robot功能包下的launch文件启动Rviz
```
$ roslaunch ur5_e_moveit_config moveit_rviz.launch config:=true
```
效果如下：

总结

本文也是基于笔者的学习和使用经验总结的，主观性较强，如果有哪些不对的地方或者不明白的地方，欢迎评论区留言交流~ Bye

Reference：

1.https://wiki.ros.org/actionlib 2.https://blog.75271.com/44975.html