ROS | 动作通信的编程实现

文章目录

• • 1.创建功能包
  • 2.节点编程与动作消息定义
  • 3.配置与编译
  • 4.话题可视化

1、创建功能包

在ROS工作空间demo05_ws的src文件夹目录下创建一个功能包，命名为homeworl_lesson2。

cd demo05_ws/

1.1、给功能包添加依赖

catkin_create_pkg homework_lesson2 roscpp rospy std_msgs   message_generation actionlib_msgs actionlib

2、节点编程与动作消息定义

客户端发送一个运动目标，模拟机器人运动到目标位置的过程，包含服务端和客户端的代码实现，要求带有实时位置反馈。

 

2.1、动作消息的定义

在homework_lesson2的工作空间下，新建一个action文件夹,再新建一个DoDishes.cation文件

 在Motion.action文件中输入以下代码，定义动作消息内容。

//定义机器人运动终点坐标endx，endy
uint32 endx
uint32 endy
---
//定义机器人动作完成标志位
uint32 Flag_Finished
---
//定义机器人当前位置坐标coordinate_x，coordinate_y
uint32 coordinate_x
uint32 coordinate_y

动作(Action)通信接口提供了五种消息定义，分别为goal、cancel、status、feedback和result，而.action文件用来定义其中三种消息，按顺序分别为goal、result和feedback，与.srv文件中的服务消息定义方式一样，使用“—”作为分隔符。

 相关资料：http://wiki.ros.org/actionlib#Tutorials

2.3、创建.cpp文件

在功能包下面的action文件夹目录下创建一个空文件move_client.cpp。

#include <actionlib/client/simple_action_client.h>
#include "homework_lesson2/DoDishesAction.h"

typedef actionlib::SimpleActionClient<homework_lesson2::DoDishesAction> Client;

// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const homework_lesson2::DoDishesResultConstPtr& result)
{
    ROS_INFO("Move (2.00000,2.00000)is finished");
    // ROS_INFO("Yay! The dishes are now clean");
    ros::shutdown();
}

// 当action激活后会调用该回调函数一次
void activeCb()
{
    ROS_INFO("Move to: (2.00000,2.00000");
    // ROS_INFO("Goal just went active");
}

// 收到feedback后调用该回调函数
void feedbackCb(const homework_lesson2::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
    homework_lesson2::DoDishesGoal goal;
    goal.dishwasher_id = 1;

    // 发送action的goal给服务器端，并且设置回调函数
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

    ros::spin();

    return 0;
}

说明：

• 头文件actionlib/client/simple_action_client.h用于实现简单的动作客户端。其中actionlib软件包提供了创建执行长期目标的服务器，以及一个客户端接口，以便将请求发送到服务器。
• 头文件action_task/MotionAction.h是由Motion.action文件编译生成，定义了动作消息内容，该文件存放在ROS_ws工作空间下的devel/include/action_task文件夹中。
• typedef actionlib::SimpleActionClient<action_task::MotionAction> Client;的作用是使用Client声明定义action_task::MotionAction类型的actionlib::SimpleActionClient类。
• doneCb(const actionlib::SimpleClientGoalState& state, const action_task::MotionResultConstPtr& result)该函数在服务器完成任务后，通知客户端调用一次，调用该回调函数后ROS系统在终端界面输出字符串”The robot has arrived at the destination.”表明动作完成，同时ros::shutdown()关闭客户端节点，终止所有开放的订阅，发布，服务及调用。
• void activeCb()该函数是在动作服务器被激活后，通知客户端开始执行任务的回调函数，调用该回调函数后ROS系统在终端界面输出字符串”Goal just went active”表明action被激活。
• void feedbackCb(const action_task::MotionFeedbackConstPtr& feedback)该函数是在动作服务器执行任务过程中，通知客户端机器人当前坐标的回调函数。const action_task::MotionFeedbackConstPtr& feedback这里存放的是由主调函数放进来的实参变量feedback的地址，通过引用传递给回调函数。调用该回调函数后ROS系统在终端界面输出当前机器人位置的横纵坐标。
• main函数中首先使用ros::init()初始化ROS节点，将该节点命名为robot_client。
• Client client(“robot_motion”, true);的作用是创建一个action_task::MotionAction类型的action客户端。Client是之前已经使用typedef重新声明定义的actionlib::SimpleActionClient<action_task::MotionAction>，client是一个action_task::MotionAction消息类型的对象，第一个参数表示action客户端与服务器之间通信的消息名称，第二个参数true表示action客户端以单线程运行。
• client.waitForServer()函数的作用是等待客户端连接到动作服务器，否则客户端将停在这里一直处于等待，不过也可以使用ros::duration()函数作为参数设置等待时间。
• action_task::MotionGoal goal是创建一个action_task::MotionGoal类的对象goal，并为goal成员endx、endy即终点坐标赋值。
• client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);的作用是发送一个目标值到action服务器，并设置多个回调函数检测goal的执行情况（action服务器和客户端分别在接收和发送goal之后会通过状态机追踪goal的状态，并采取相应的处理）。
• ros::spin();的作用是让程序进入自循环的挂起状态，从而让程序以最好的效率接收客户端的请求并调用回调函数。                                                                                                                           

  2.5 action服务器编程                                                                                  

• 在src文件夹下再创建一个空文件robotserver.cpp，输入以下代码。   

• #include <ros/ros.h>
  #include <actionlib/server/simple_action_server.h>
  #include "homework_lesson2/DoDishesAction.h"
  
  typedef actionlib::SimpleActionServer<homework_lesson2::DoDishesAction> Server;
  
  // 收到action的goal后调用该回调函数
  void execute(const homework_lesson2::DoDishesGoalConstPtr& goal, Server* as)
  {
      ros::Rate r(1);
      homework_lesson2::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;
  }