【ROS2实操三】动作通信

概念

关于action通信,我们先从之前导航中的应用场景开始介绍,描述如下:

机器人导航到某个目标点,此过程需要一个节点A发布目标信息,然后一个节点B接收到请求并控制移动,最终响应目标达成状态信息。

乍一看,这好像是服务通信实现,因为需求中要A发送目标,B执行并返回结果,这是一个典型的基于请求响应的应答模式,不过,如果只是使用基本的服务通信实现,存在一个问题:导航是一个过程,是耗时操作,如果使用服务通信,那么只有在导航结束时,才会产生响应结果,而在导航过程中,节点A是不会获取到任何反馈的,从而可能出现程序"假死"的现象,过程的不可控意味着不良的用户体验,以及逻辑处理的缺陷(比如:导航中止的需求无法实现)。更合理的方案应该是:导航过程中,可以连续反馈当前机器人状态信息,当导航终止时,再返回最终的执行结果。在ROS中,该实现策略称之为:action 通信。

动作通信适用于长时间运行的任务。就结构而言动作通信由目标、反馈和结果三部分组成;就功能而言动作通信类似于服务通信,动作客户端可以发送请求到动作服务端,并接收动作服务端响应的最终结果,不过动作通信可以在请求响应过程中获取连续反馈,并且也可以向动作服务端发送任务取消请求;就底层实现而言动作通信是建立在话题通信和服务通信之上的,目标发送实现是对服务通信的封装,结果的获取也是对服务通信的封装,而连续反馈则是对话题通信的封装。

作用:一般适用于耗时的请求响应场景,用以获取连续的状态反馈。

一、 案例以及案例分析

需求:编写动作通信,动作客户端提交一个整型数据N,动作服务端接收请求数据并累加1-N之间的所有整数,将最终结果返回给动作客户端,且每累加一次都需要计算当前运算进度并反馈给动作客户端。

在上述案例中,需要关注的要素有三个:动作客户端;动作服务端;消息载体。

1.准备工作

终端下进入工作空间的src目录,调用如下两条命令分别创建C++功能包和Python功能包。

ros2 pkg create cpp03_action --build-type ament_cmake --dependencies rclcpp rclcpp_action base_interfaces_demo
ros2 pkg create py03_action --build-type ament_python --dependencies rclpy base_interfaces_demo

2.动作通信接口消息

定义动作接口消息与定义话题或服务接口消息流程类似,我们可以编写一个action文件,该文件中包含请求数据(一个整型字段)、响应数据(一个整型字段)和连续反馈数据(一个浮点型字段)。

1.创建并编辑 .action 文件

功能包base_interfaces_demo下新建action文件夹,action文件夹下新建Progress.action。

int64 num
---
int64 sum
---
float64 progress
2.编辑配置文件
1.package.xml

如果单独构建action功能包,需要在package.xml中需要添加一些依赖包,具体内容如下:

XML 复制代码
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>

当前使用的是 base_interfaces_demo 功能包,已经为 msg 、srv 文件添加过了一些依赖,所以 package.xml 中添加如下内容即可:

XML 复制代码
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
2.CMakeLists.txt

如果是新建的功能包,与之前定义msg、srv文件同理,为了将.action文件转换成对应的C++和Python代码,还需要在CMakeLists.txt 中添加如下配置:

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/Progress.action"
)

不过,我们当前使用的base_interfaces_demo包,那么只需要修改rosidl_generate_interfaces函数即可,修改后的内容如下:

bash 复制代码
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Student.msg"
  "srv/AddInts.srv"
  "action/Progress.action"
)
3.编译

终端中进入当前工作空间,编译功能包:

colcon build --packages-select base_interfaces_demo
4.测试

编译完成之后,在工作空间下的 install 目录下将生成Progress.action文件对应的C++和Python文件,我们也可以在终端下进入工作空间,通过如下命令查看文件定义以及编译是否正常:

. install/setup.bash
ros2 interface show base_interfaces_demo/action/Progress

二、动作通信(C++)

1.动作服务端实现

功能包cpp03_action的src目录下,新建C++文件demo01_action_server.cpp。

cpp 复制代码
/*  
  需求:编写动作服务端实习,可以提取客户端请求提交的整型数据,并累加从1到该数据之间的所有整数以求和,
       每累加一次都计算当前运算进度并连续反馈回客户端,最后,在将求和结果返回给客户端。
  步骤:
    1.包含头文件;
    2.初始化 ROS2 客户端;
    3.定义节点类;
      3-1.创建动作服务端;
      3-2.处理请求数据;
      3-3.处理取消任务请求;
      3-4.生成连续反馈。
    4.调用spin函数,并传入节点对象指针;
    5.释放资源。

*/
// 1.包含头文件;
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"

using namespace std::placeholders;
using base_interfaces_demo::action::Progress;
using GoalHandleProgress = rclcpp_action::ServerGoalHandle<Progress>;

// 3.定义节点类;
class MinimalActionServer : public rclcpp::Node
{
public:

  explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("minimal_action_server", options)
  {
    // 3-1.创建动作服务端;
    this->action_server_ = rclcpp_action::create_server<Progress>(
      this,
      "get_sum",
      std::bind(&MinimalActionServer::handle_goal, this, _1, _2),
      std::bind(&MinimalActionServer::handle_cancel, this, _1),
      std::bind(&MinimalActionServer::handle_accepted, this, _1));
    RCLCPP_INFO(this->get_logger(),"动作服务端创建,等待请求...");
  }

private:
  rclcpp_action::Server<Progress>::SharedPtr action_server_;

  // 3-2.处理请求数据;
  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const Progress::Goal> goal)
  {
    (void)uuid;
    RCLCPP_INFO(this->get_logger(), "接收到动作客户端请求,请求数字为 %ld", goal->num);
    if (goal->num < 1) {
      return rclcpp_action::GoalResponse::REJECT;
    }
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  // 3-3.处理取消任务请求;
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleProgress> goal_handle)
  {
    (void)goal_handle;
    RCLCPP_INFO(this->get_logger(), "接收到任务取消请求");
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void execute(const std::shared_ptr<GoalHandleProgress> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "开始执行任务");
    rclcpp::Rate loop_rate(10.0);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Progress::Feedback>();
    auto result = std::make_shared<Progress::Result>();
    int64_t sum= 0;
    for (int i = 1; (i <= goal->num) && rclcpp::ok(); i++) {
      sum += i;
      // Check if there is a cancel request
      if (goal_handle->is_canceling()) {
        result->sum = sum;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "任务取消");
        return;
      }
      feedback->progress = (double_t)i / goal->num;
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "连续反馈中,进度:%.2f", feedback->progress);

      loop_rate.sleep();
    }

    if (rclcpp::ok()) {
      result->sum = sum;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "任务完成!");
    }
  }

  // 3-4.生成连续反馈。
  void handle_accepted(const std::shared_ptr<GoalHandleProgress> goal_handle)
  {
    std::thread{std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach();
  }
}; 

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端;
  rclcpp::init(argc, argv);
  // 4.调用spin函数,并传入节点对象指针;
  auto action_server = std::make_shared<MinimalActionServer>();
  rclcpp::spin(action_server);
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

2.动作客户端实现

功能包cpp03_action的src目录下,新建C++文件demo02_action_client.cpp。

cpp 复制代码
/*  
  需求:编写动作客户端实现,可以提交一个整型数据到服务端,并处理服务端的连续反馈以及最终返回结果。
  步骤:
    1.包含头文件;
    2.初始化 ROS2 客户端;
    3.定义节点类;
      3-1.创建动作客户端;
      3-2.发送请求;
      3-3.处理目标发送后的反馈;
      3-4.处理连续反馈;
      3-5.处理最终响应。
    4.调用spin函数,并传入节点对象指针;
    5.释放资源。
*/
// 1.包含头文件;
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"

using base_interfaces_demo::action::Progress;
using GoalHandleProgress = rclcpp_action::ClientGoalHandle<Progress>;
using namespace std::placeholders;

// 3.定义节点类;
class MinimalActionClient : public rclcpp::Node
{
public:

  explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
  : Node("minimal_action_client", node_options)
  {
    // 3-1.创建动作客户端;
    this->client_ptr_ = rclcpp_action::create_client<Progress>(this,"get_sum");
  }

  // 3-2.发送请求;
  void send_goal(int64_t num)
  {

    if (!this->client_ptr_) {
      RCLCPP_ERROR(this->get_logger(), "动作客户端未被初始化。");
    }

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(), "服务连接失败!");
      return;
    }

    auto goal_msg = Progress::Goal();
    goal_msg.num = num;
    RCLCPP_INFO(this->get_logger(), "发送请求数据!");

    auto send_goal_options = rclcpp_action::Client<Progress>::SendGoalOptions();
    send_goal_options.goal_response_callback =std::bind(&MinimalActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =std::bind(&MinimalActionClient::result_callback, this, _1);
    auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<Progress>::SharedPtr client_ptr_;

  // 3-3.处理目标发送后的反馈;
  void goal_response_callback(GoalHandleProgress::SharedPtr goal_handle)
  {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "目标请求被服务器拒绝!");
    } else {
      RCLCPP_INFO(this->get_logger(), "目标被接收,等待结果中");
    }
  }

  // 3-4.处理连续反馈;
  void feedback_callback(GoalHandleProgress::SharedPtr,const std::shared_ptr<const Progress::Feedback> feedback)
  {
    int32_t progress = (int32_t)(feedback->progress * 100);
    RCLCPP_INFO(this->get_logger(), "当前进度: %d%%", progress);
  }

  // 3-5.处理最终响应。
  void result_callback(const GoalHandleProgress::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "任务被中止");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "任务被取消");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "未知异常");
        return;
    }

    RCLCPP_INFO(this->get_logger(), "任务执行完毕,最终结果: %ld", result.result->sum);
  }
}; 

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端;
  rclcpp::init(argc, argv);

  // 4.调用spin函数,并传入节点对象指针;
  auto action_client = std::make_shared<MinimalActionClient>();
  action_client->send_goal(10);
  rclcpp::spin(action_client);
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

3.编辑配置文件

1.packages.xml

在创建功能包时,所依赖的功能包已经自动配置了,配置内容如下:

XML 复制代码
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>base_interfaces_demo</depend>
2.CMakeLists.txt

CMakeLists.txt中服务端和客户端程序核心配置如下:

bash 复制代码
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(base_interfaces_demo REQUIRED)

add_executable(demo01_action_server src/demo01_action_server.cpp)
ament_target_dependencies(
  demo01_action_server
  "rclcpp"
  "rclcpp_action"
  "base_interfaces_demo"
)

add_executable(demo02_action_client src/demo02_action_client.cpp)
ament_target_dependencies(
  demo02_action_client
  "rclcpp"
  "rclcpp_action"
  "base_interfaces_demo"
)

install(TARGETS 
  demo01_action_server
  demo02_action_client
  DESTINATION lib/${PROJECT_NAME})

4.编译

终端中进入当前工作空间,编译功能包:

colcon build --packages-select cpp03_action

5.执行

当前工作空间下,启动两个终端,终端1执行动作服务端程序,终端2执行动作客户端程序。

终端1输入如下指令:

. install/setup.bash
ros2 run cpp03_action demo01_action_server

终端2输入如下指令:

. install/setup.bash
ros2 run cpp03_action demo02_action_client

三、动作通信(Python)

1.动作服务端实现

功能包py03_action的py03_action目录下,新建Python文件demo01_action_server_py.py。

python 复制代码
"""  
    需求:编写动作服务端实习,可以提取客户端请求提交的整型数据,并累加从1到该数据之间的所有整数以求和,
       每累加一次都计算当前运算进度并连续反馈回客户端,最后,在将求和结果返回给客户端。
    步骤:
        1.导包;
        2.初始化 ROS2 客户端;
        3.定义节点类;
            3-1.创建动作服务端;
            3-2.生成连续反馈;
            3-3.生成最终响应。
        4.调用spin函数,并传入节点对象;
        5.释放资源。
"""

# 1.导包;
import time
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node

from base_interfaces_demo.action import Progress

# 3.定义节点类;
class ProgressActionServer(Node):

    def __init__(self):
        super().__init__('progress_action_server')
        # 3-1.创建动作服务端;
        self._action_server = ActionServer(
            self,
            Progress,
            'get_sum',
            self.execute_callback)
        self.get_logger().info('动作服务已经启动!')

    def execute_callback(self, goal_handle):
        self.get_logger().info('开始执行任务....')


        # 3-2.生成连续反馈;
        feedback_msg = Progress.Feedback()

        sum = 0
        for i in range(1, goal_handle.request.num + 1):
            sum += i
            feedback_msg.progress = i / goal_handle.request.num
            self.get_logger().info('连续反馈: %.2f' % feedback_msg.progress)
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(1)

        # 3-3.生成最终响应。
        goal_handle.succeed()
        result = Progress.Result()
        result.sum = sum
        self.get_logger().info('任务完成!')

        return result


def main(args=None):

    # 2.初始化 ROS2 客户端;
    rclpy.init(args=args)

    # 4.调用spin函数,并传入节点对象;
    Progress_action_server = ProgressActionServer()
    rclpy.spin(Progress_action_server)

    # 5.释放资源。
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2.动作客户端实现

功能包py03_action的py03_action目录下,新建Python文件demo02_action_client_py.py。

python 复制代码
"""  
    需求:编写动作客户端实现,可以提交一个整型数据到服务端,并处理服务端的连续反馈以及最终返回结果。
    步骤:
        1.导包;
        2.初始化 ROS2 客户端;
        3.定义节点类;
            3-1.创建动作客户端;
            3-2.发送请求;
            3-3.处理目标发送后的反馈;
            3-4.处理连续反馈;
            3-5.处理最终响应。
        4.调用spin函数,并传入节点对象;
        5.释放资源。

"""
# 1.导包;
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from base_interfaces_demo.action import Progress

# 3.定义节点类;
class ProgressActionClient(Node):

    def __init__(self):
        super().__init__('progress_action_client')
        # 3-1.创建动作客户端;
        self._action_client = ActionClient(self, Progress, 'get_sum')

    def send_goal(self, num):
        # 3-2.发送请求;
        goal_msg = Progress.Goal()
        goal_msg.num = num
        self._action_client.wait_for_server()
        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        # 3-3.处理目标发送后的反馈;
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('请求被拒绝')
            return

        self.get_logger().info('请求被接收,开始执行任务!')

        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    # 3-5.处理最终响应。
    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info('最终计算结果:sum = %d' % result.sum)
        # 5.释放资源。
        rclpy.shutdown()

    # 3-4.处理连续反馈;
    def feedback_callback(self, feedback_msg):
        feedback = (int)(feedback_msg.feedback.progress * 100)
        self.get_logger().info('当前进度: %d%%' % feedback)


def main(args=None):

    # 2.初始化 ROS2 客户端;
    rclpy.init(args=args)
    # 4.调用spin函数,并传入节点对象;

    action_client = ProgressActionClient()
    action_client.send_goal(10)
    rclpy.spin(action_client)

    # rclpy.shutdown()


if __name__ == '__main__':
    main()

3.编辑配置文件

1.package.xml

在创建功能包时,所依赖的功能包已经自动配置了,配置内容如下:

XML 复制代码
<depend>rclpy</depend>
<depend>base_interfaces_demo</depend>
2.setup.py

entry_points字段的console_scripts中添加如下内容:

python 复制代码
entry_points={
    'console_scripts': [
        'demo01_action_server_py = py03_action.demo01_action_server_py:main',
        'demo02_action_client_py = py03_action.demo02_action_client_py:main'
    ],
},

4.编译

终端中进入当前工作空间,编译功能包:

colcon build --packages-select py03_action

5.执行

当前工作空间下,启动两个终端,终端1执行动作服务端程序,终端2执行动作客户端程序。

终端1输入如下指令:

. install/setup.bash
ros2 run py03_action demo01_action_server_py

终端2输入如下指令:

. install/setup.bash
ros2 run py03_action demo02_action_client_py

下期见!

相关推荐
黑客学长-刘备几秒前
终于有人把网络安全就业方向一口气讲清了(非常详细)零基础入门到精通,收藏这一篇就够了
java·运维·服务器·网络·python·安全·web安全
Json_181790144802 分钟前
小红书笔记详情API接口系列(概述到示例案例)
开发语言·python
云卓科技4 分钟前
无人机之任务分配算法篇
科技·算法·机器人·无人机·交互·制造
云卓科技7 分钟前
无人机之目标检测算法篇
人工智能·科技·算法·目标检测·计算机视觉·机器人·无人机
Loong_DQX1 小时前
【flask】 前后端通信方式 原生js的ajax,总结
后端·python·flask
大白的编程日记.1 小时前
【C++笔记】list结构剖析及其模拟实现
c++·笔记·list
Python大数据分析@1 小时前
Python中除了matplotlib外还有哪些数据可视化的库?
python·信息可视化·matplotlib
IT90902 小时前
Python-数据爬取(爬虫)
开发语言·爬虫·python
Demons_皮2 小时前
python:ADB通过包名打开应用
开发语言·python·adb
AlexMercer10122 小时前
[C++ 核心编程]笔记 4.2.6 初始化列表
开发语言·数据结构·c++·笔记·算法