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

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

前言

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

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

  moveit！的关系图如下所示：   我们可以使用探索3中的例程来分析一下，我们需要处理什么数据。  

1. 打开webots，打开探索3中创建的世界
2. 打开以下launch文件，设置所必须的ROS参数并且连接Webots中的UR机械臂

   $ roslaunch ur_e_webots ur5e.launch
   

3. 连接成功后，查看当前系统中的话题列表，如下所示。

• /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）的工作模式，如下图所示：     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，他的作用是：

1. 接受机器人产生的传感器数据，并且发布各个关节的关节状态话题/joint_states。
2. 处理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）

 

1. 先看一下webots发布了什么service,可以从下面的图看到ur5e发布了位置传感器的使能服务。
2. 由于发布/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]);
    }
   }
   

3. 使能完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.发布效果    

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

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

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

1. 在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]);
   }
   

2. 我们知道，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
• 发布/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://www.guyuehome.com/908