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

83
0
2021年2月14日 09时40分

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中的例程来分析一下,我们需要处理什么数据。

 

  1. 打开webots,打开探索3中创建的世界
  2. 打开以下launch文件,设置所必须的ROS参数并且连接Webots中的UR机械臂
    $ roslaunch ur_e_webots ur5e.launch
    
  3. 连接成功后,查看当前系统中的话题列表,如下所示。
    ROS-moveit!探索(4)webots和moveit联合控制[2]插图(1)
  • /follow_joint_trajectory开头的topic是Moveit!最终规划发布的action(_第二节介绍_)消息,由机器人控制器端接受该消息后控制机器人完成运动。
  • /joint_states由机器人控制器端发布,主要发布机器人的当前状态信息,rostopic echo一下。
    ROS-moveit!探索(4)webots和moveit联合控制[2]插图(2)
    由此也是可以看出:

    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]插图(3)

 

client和server之间通过actionlib定义的“action protocol”进行通讯。这种通讯协议是基于ROS的消息机制实现的,为用户提供了client和server的接口,接口如下图所示:

 

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

 

在上边的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发布了位置传感器的使能服务。
    ROS-moveit!探索(4)webots和moveit联合控制[2]插图(5)
  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话题,如下所示:
    ROS-moveit!探索(4)webots和moveit联合控制[2]插图(6)

 

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

 

``` 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]插图(7)

 

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
    ROS-moveit!探索(4)webots和moveit联合控制[2]插图(8)

  • 发布/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
    

    效果如下:

    ROS-moveit!探索(4)webots和moveit联合控制[2]插图(9)

总结

 

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

Bye

Reference:

1.https://wiki.ros.org/actionlib
2.https://www.guyuehome.com/908

发表评论

后才能评论