ROS进阶功能

一、action

action通讯机制：一种带有连续反馈的上层通讯机制，底层基于ROS话题通讯

在很多情况下，话题（Topic）和服务（Service），但在很多场景下往往满足不了需求。例如：在机械臂控制中，如果用话题发布运动目标，由于话题是单向通信，则需要订阅一个话题来获得机器人运动过程中的状态反馈；如果由服务发布运动目标，虽然可以获得一次反馈信息，但是对于控制来说数据很少，而当反馈没有收到时，只能做等待操作

1.1概念

action是一种类似于Service的问答通讯机制，不同之处在于action带有连续的反馈，可以不断的反馈任务进度，也可以在任务执行过程中终止运行

1.2工作机制

action采用客户端/服务端（Client/Server）的工作模式

客户端与服务端之间通过actionlib定义的action protocol进行通信

这种通讯机制基于ROS消息机制实现，为用户提供了如下所示Client/Server接口

action提供的Topics列表：

• goal：发布任务目标
• cancel：请求取消任务
• status：通知Client当前任务
• feedback：周期反馈任务运行的监控数据
• result：向Client发送任务的执行结果，只发布一次

1.3定义

action通过.action文件进行定义，放置在action文件夹下，文件格式如下：

# 定义目标信息goal
uint32 dishwasher_id  # 指定我们要使用的dishwasher
---
# 定义结果信息result
uint32 total_dishes_cleaned
---
# 定义周期反馈信息feedback
float32 percent_complete

在一个action定义中需要定义三个部分：goal、result、feedback

创建.action文件之后，还需要将该文件进行编译，和自定义message和service文件一样，需要在CMakeLists.txt文件中添加如下规则：

find_package(catkin REQUIRED genmsg actionlib_msgs actionlib)
add_action_files(DIRECTORY action FILES DoDishes.action)
generate_messages(DEPENDENCIES actionlib_msgs)

在功能包中的package.xml中添加如下配置：

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<run_depend>actionlib</run_depend>
<run_depend>actionlib_msgs</run_depend>

编译后可产生一系列.msg文件

从编译后的文件可以看出，action是基于message的，更高层的一种通讯机制

1.4实现action通讯

1.4.1客户端

#include <actionlib/client/simple_action_client.h>
#include "action_tutorials/DoDishesAction.h"
 
typedef actionlib::SimpleActionClient<action_tutorials::DoDishesAction> Client;
 
// 当action完成后会调用次回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const action_tutorials::DoDishesResultConstPtr& result)
{
    ROS_INFO("Yay! The dishes are now clean");
    ros::shutdown();
}
 
// 当action激活后会调用次回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}
 
// 收到feedback后调用的回调函数
void feedbackCb(const action_tutorials::DoDishesFeedbackConstPtr& feedback)
{
    ROS_INFO(" percent_complete : %f ", feedback->percent_complete);
}
 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "do_dishes_client");
 
    // 定义一个客户端
    Client client("do_dishes", true);
 
    // 等待服务器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");
 
    // 创建一个action的goal
    action_tutorials::DoDishesGoal goal;
    goal.dishwasher_id = 1;
 
    // 发送action的goal给服务器端，并且设置回调函数
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);
 
    ros::spin();
 
    return 0;
}

1.4.2服务端

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "action_tutorials/DoDishesAction.h"
 
typedef actionlib::SimpleActionServer<action_tutorials::DoDishesAction> Server;
 
// 收到action的goal后调用的回调函数
void execute(const action_tutorials::DoDishesGoalConstPtr& goal, Server* as)
{
    ros::Rate r(1);
    action_tutorials::DoDishesFeedback feedback;
 
    ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);
 
    // 假设洗盘子的进度，并且按照1hz的频率发布进度feedback
    for(int i=1; i<=10; i++)
    {
        feedback.percent_complete = i * 10;
        as->publishFeedback(feedback);
        r.sleep();
    }
 
    // 当action完成后，向客户端返回结果
    ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
    as->setSucceeded();
}
 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "do_dishes_server");
    ros::NodeHandle n;
 
    // 定义一个服务器
    Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
 
    // 服务器开始运行
    server.start();
 
    ros::spin();
 
    return 0;
}

1.4.3运行效果

使用如下命令运行客户端节点，由于服务器没有启动，客户端会保持等待状态

$ rosrun action_tutorials DoDisher_client

后运行服务端节点，客户端可以看到反馈的进度信息

$ rosrun action_tutorials DoDisher_server

二、plugin

ROS开发中常常需要使用到plugin插件，在使用插件时，不需要考虑plugin类的连接位置，只需要将插件注册到pluginlib中即可动态加载

2.1工作原理

2.2实现插件

利用C++多态特性，不同插件只需要使用统一的接口就可以替换使用，选择需要使用的插件即可拓展相应的功能

一般来说，要实现一个插件，主要分为如下几步：

• 创建基类，定义统一接口（如果基于现有积累实现插件，则不需要这个步骤）
• 创建plugin类，继承基类，实现统一的接口
• 注册插件
• 编译生成插件的动态链接库
• 将插件加入ROS中

首先创建一个pluginlib_tutorials功能包，注意添加依赖pluginlib

$ catkin_create_pkg plugin_tutorials roscpp pluginlib

2.3创建基类

创建一个polygon的基类（polygon_base.h），定义了一些简单的接口，需要注意的是initialize()这个接口的作用

#ifndef PLUGINLIB_TUTORIALS_POLYGON_BASE_H_
#define PLUGINLIB_TUTORIALS_POLYGON_BASE_H_
 
namespace polygon_base
{
  class RegularPolygon
  {
    public:
      //pluginlib要求构造函数不能带有参数，所以定义initialize来完成需要初始化的工作
      virtual void initialize(double side_length) = 0;
 
      //计算面积的接口函数
      virtual double area() = 0;
 
      virtual ~RegularPolygon(){}
 
    protected:
      RegularPolygon(){}
  };
};
#endif

2.4创建plugin类

接下来创建rectangle_plugin和triangle_plugin类（polygon_plugins.h），实现基类的接口，也可以添加plugin自己需要的接口

#ifndef PLUGINLIB_TUTORIALS_POLYGON_PLUGINS_H_
#define PLUGINLIB_TUTORIALS_POLYGON_PLUGINS_H_
#include <pluginlib_tutorials/polygon_base.h>
#include <cmath>
 
namespace polygon_plugins
{
  class Triangle : public polygon_base::RegularPolygon
  {
    public:
      Triangle() : side_length_() {}
 
      // 初始化边长
      void initialize(double side_length)
      {
        side_length_ = side_length;
      }
 
      double area()
      {
        return 0.5 * side_length_ * getHeight();
      }
 
      // Triangle类自己的接口
      double getHeight()
      {
        return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2)));
      }
 
    private:
      double side_length_;
  };
 
  class Square : public polygon_base::RegularPolygon
  {
    public:
      Square() : side_length_() {}
 
      // 初始化边长
      void initialize(double side_length)
      {
        side_length_ = side_length;
      }
 
      double area()
      {
        return side_length_ * side_length_;
      }
 
    private:
      double side_length_;
 
  };
};
#endif

2.5注册插件

接下来我们还需要创建一个cpp文件（polygon_plugins.cpp），来注册插件，声明我们创建了两个插件：

//包含pluginlib的头文件，使用pluginlib的宏来注册插件
#include <pluginlib/class_list_macros.h>
#include <pluginlib_tutorials/polygon_base.h>
#include <pluginlib_tutorials/polygon_plugins.h>
 
//注册插件，宏参数：plugin的实现类，plugin的基类
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon);
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon);

2.6编译动态链接库

为了编译该功能包，需要修改CMakefile.txt文件，加入如下两条编译规则：

include_directories(include)
add_library(polygon_plugins src/polygon_plugins.cpp)

2.7将插件加入ROS

为了便于使用插件，还需要编写xml文件将plugin加入ROS中

这里需要创建和修改功能包根目录下的两个xml文件

2.7.1创建plugin_tuttorials/polygon_plugins.xml

<library path="lib/libpluginlib_tutorials">
  <class name="pluginlib_tutorials/regular_triangle" type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon">
    <description>This is a triangle plugin.</description>
  </class>
  <class name="pluginlib_tutorials/regular_square" type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon">
    <description>This is a square plugin.</description>
  </class>
</library>

该xml文件主要描述了plugin的动态库路径、实现类、基类、功能描述等信息

2.7.2修改plugin_tuttorials/package.xml

添加如下代码

<export>
  <pluginlib_tutorials plugin="${prefix}/polygon_plugins.xml" /> 
</export>

然后可以通过如下命令查看功能包的插件路径

$ rospack plugins --attrib=plugin pluginlib_tutorials

2.8调用插件

#include <boost/shared_ptr.hpp>
 
#include <pluginlib/class_loader.h>
#include <pluginlib_tutorials/polygon_base.h>
 
int main(int argc, char** argv)
{
  // 创建一个ClassLoader，用来加载plugin
  pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("pluginlib_tutorials", "polygon_base::RegularPolygon");
 
  try
  {
    // 加载Triangle插件类，路径在polygon_plugins.xml中定义
    boost::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createInstance("pluginlib_tutorials/regular_triangle");
 
    // 初始化边长
    triangle->initialize(10.0);
 
    ROS_INFO("Triangle area: %.2f", triangle->area());
  }
  catch(pluginlib::PluginlibException& ex)
  {
    ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
  }
 
  try
  {
    boost::shared_ptr<polygon_base::RegularPolygon> square = poly_loader.createInstance("pluginlib_tutorials/regular_square");
    square->initialize(10.0);
 
    ROS_INFO("Square area: %.2f", square->area());
  }
  catch(pluginlib::PluginlibException& ex)
  {
    ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
  }
 
  return 0;
}