当前位置:   article > 正文

ROS高效进阶第六章 -- Ros Action通信机制

ros action

1 背景和资料

从本文开始我们用三篇文章学习机器人SLAM建图与自主导航,本文先单独介绍下ROS的Action通信机制,后面自主导航的样例将用到ros action。在之前的ROS高效入门系列文章里,我们详细学习了ros的topic和service通信机制,而ros的action是第三种通信机制。
本文参考资料如下:
(1)《ROS机器人开发实践》胡春旭 第12章第一节
(2)chatGPT
本系列博客汇总:ROS高效进阶系列

2 正文

2.1 ros action通信机制讲解

(1)ros topic是异步通信,是广播通信,通信上下游没有应答机制,类似udp协议。而ros service是一种带有应答(请求和回复)的通信机制,采用C/S模式,是同步通信,是一对一的,类似tcp协议。
(2)ros action类似service,也有应答机制,也是C/S模式,不同之处在于ros action带有连续反馈。server端可以不停的向client端发布任务进展,直到任务结束时发布一次任务处理结论,而client可以在任务执行中间发送任务终止请求。很明显,ros action适合大型任务的通信沟通,通常需要持续一段时间。
(3)ros action依赖ros的actionlib库,基于ros msg进行封装的一种更高级的通信方式。其中,client与server端有如下五个接口:
在这里插入图片描述

goal:client使用goal接口向server发布任务
cancel:任务中途,client使用cancel接口向server发布任务中止请求
status:任务结束时,server向client反馈任务状态,如SUCCEEDED,ABORTED
result:任务结束时,server向client反馈任务结果,比如刷了多少个盘子
feedback:任务过程中,server向client反馈任务处理进展,比如完成了百分之多少
  • 1
  • 2
  • 3
  • 4
  • 5

2.2 ros action样例之洗碗机刷盘子

(1)我们创建的样例逻辑是:客户端向服务端指定一个洗碗机刷盘子,服务端拿到洗碗机号,去刷盘子,定期反馈进度,最后反馈盘子个数。任务过程中,客户端可能会向服务端发送任务中止请求,服务端收到中止请求,返回任务状态和已经刷的盘子个数。
创建 action_example 样例及相关文件:

cd ~/catkin_ws/src
catkin_create_pkg action_sample actionlib actionlib_msgs std_msgs rospy roscpp message_generation message_runtime

cd action_sample 
mkdir action launch scripts
touch action/DoDishes.action
touch launch/start_cpp.launch launch/start_py.launch
touch scripts/DoDishes_client.py scripts/DoDishes_server.py
touch src/DoDishes_client.cpp src/DoDishes_server.cpp
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

(2)首先是定义action文件,通常是三段式(goal,result,feedback),如这里的 DoDishes.action:

# Goal,内容是洗碗机号
uint32 dishwasher_id
---
# Result,刷盘子的个数
uint32 total_dishes_cleaned
---
# Feedback,反馈一般是进度
float32 percent_complete
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

(3)DoDishes_client.cpp

#include <actionlib/client/simple_action_client.h>
#include "action_sample/DoDishesAction.h"
// actionlib::SimpleClientGoalState 是拓扑图中的 status
// action_sample::DoDishesResultConstPtr 是拓扑图中的 result
// 服务端完成任务并反馈后,客户端调用一次doneCb
void doneCb(const actionlib::SimpleClientGoalState& state,
            const action_sample::DoDishesResultConstPtr& result) {
  ROS_INFO("%d dishes is clean, state is %s", result->total_dishes_cleaned, state.toString().c_str());
  ros::shutdown();
}
// 服务端接收任务并启动后,客户端调用一次activeCb
void activeCb() {
  ROS_INFO("job is active");
}
// 服务端周期反馈进度,客户端周期调用feedbackCb
void feedbackCb(const action_sample::DoDishesFeedbackConstPtr& feedback) {
  ROS_INFO("percent is %f", feedback->percent_complete);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "do_dishes_client");
  // action的类型名是DoDishes.action文件的名字加Action,即DoDishesAction
  // 客户端与服务端通过action名链接,即"do_dishes"
  // true表示客户端启动后,就自动尝试连接服务端,理论上这里是true,就不需要调用下面的waitForServer()
  actionlib::SimpleActionClient<action_sample::DoDishesAction> client("do_dishes", true);

  ROS_INFO("waiting for action server to start.");
  client.waitForServer();
  ROS_INFO("action server started, sending goal.");

  action_sample::DoDishesGoal goal;
  goal.dishwasher_id = 1;
  client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

  // sleep(4);
  // client.cancelGoal();    // 任务取消命令

  ros::spin();
  return 0;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40

(4)DoDishes_server.cpp

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "action_sample/DoDishesAction.h"
// action_sample::DoDishesGoalConstPtr是拓扑中的goal
void execute(const action_sample::DoDishesGoalConstPtr& goal, actionlib::SimpleActionServer<action_sample::DoDishesAction>* server) {
  ros::Rate rate(1);
  // 这是拓扑中的feedback
  // 这是拓扑中的result
  action_sample::DoDishesFeedback feedback;
  action_sample::DoDishesResult result;
  ROS_INFO("dishwasher %d is working", goal->dishwasher_id);

  int i = 0;
  for (i = 0; i < 10; i++) {
    feedback.percent_complete = i * 10;
    // 调用publishFeedback()周期反馈进度
    server->publishFeedback(feedback);
    rate.sleep();
    // 如果任务被取消,则执行这里,反馈取消状态和中途执行结果
    if (server->isPreemptRequested()) {
      ROS_INFO("The goal is canceled");
      result.total_dishes_cleaned = i*10;
      server->setPreempted(result);
      return;
    }
  }
  ROS_INFO("dishwasher %d finish working", goal->dishwasher_id);
  result.total_dishes_cleaned = i*10;
  // 任务成功,执行setSucceeded(),反馈成功状态和结果
  server->setSucceeded(result);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "do_dishes_server");
  ros::NodeHandle nh;
  // "do_dishes"必须与客户端的名称相同
  // 接收到任务后,执行execute()回调函数
  // false表示需要调用start(),手动启动服务端
  // 这里在创建server句柄的同时,又把server句柄绑定给了execute(),用法比较特殊
  actionlib::SimpleActionServer<action_sample::DoDishesAction> server(nh, "do_dishes", std::bind(&execute, std::placeholders::_1, &server), false);
  server.start();
  ros::spin();
  return 0;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44

(5)DoDishes_client.py(注释类似DoDishes_client.cpp,请对照着看,不写了)

#! /usr/bin/env python3
import rospy
import actionlib
from action_sample.msg import DoDishesAction, DoDishesGoal

def done_cb(state, result):
  rospy.loginfo("%d dishes is clean, state is %s" %(result.total_dishes_cleaned, str(state)))
  rospy.signal_shutdown('Normal shutdown')

def active_cb():
  rospy.loginfo("job is action")
  
def feedback_cb(feedback):
  rospy.loginfo("percent is %f" %feedback.percent_complete)

def main():
  rospy.init_node("do_dishes_client")
  client = actionlib.SimpleActionClient("do_dishes", DoDishesAction)
  rospy.loginfo("waiting for action server to start")
  client.wait_for_server()
  rospy.loginfo("action server started, sending goal")
  goal = DoDishesGoal()
  goal.dishwasher_id = 1
  client.send_goal(goal, done_cb=done_cb, active_cb=active_cb, feedback_cb=feedback_cb)
  
  rospy.sleep(4)
  client.cancel_goal()
  rospy.spin()

if __name__ == "__main__":
  main()
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31

(6)DoDishes_server.py

#! /usr/bin/env python3
import rospy
import actionlib
from action_sample.msg import DoDishesAction, DoDishesGoal, DoDishesFeedback, DoDishesResult
// 采用类解决execute_cb()绑定server句柄的问题
class DishWasherServer(object):
  def __init__(self, action_name):
    self._action_name = action_name
    self._server = actionlib.SimpleActionServer(self._action_name, DoDishesAction, execute_cb=self.execute_cb, auto_start=False)

  def start_server(self):
    self._server.start()
    
  def execute_cb(self, goal):
    rate = rospy.Rate(1)
    rospy.loginfo("dishwasher %d is working" %goal.dishwasher_id)
    
    feedback = DoDishesFeedback()
    result = DoDishesResult()
    for i in range(10):
      feedback.percent_complete = i * 10
      self._server.publish_feedback(feedback)
      rate.sleep()
      if self._server.is_preempt_requested():
        rospy.loginfo("the goal is canceled")
        result.total_dishes_cleaned = i * 10
        self._server.set_preempted(result)
        return
    
    rospy.loginfo("dishwasher %d finish working" %goal.dishwasher_id)
    result.total_dishes_cleaned = (i+1) * 10
    self._server.set_succeeded(result)
    
def main():
  rospy.init_node("do_dishes_server")
  dish_washer_server = DishWasherServer("do_dishes")
  dish_washer_server.start_server()
  rospy.spin()

if __name__ == "__main__":
  main()
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41

(7)CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(action_sample)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  actionlib
  actionlib_msgs
)
add_action_files(
  DIRECTORY action
  FILES DoDishes.action
)
generate_messages(
  DEPENDENCIES
  std_msgs
  actionlib_msgs
)
catkin_package(
  CATKIN_DEPENDS actionlib actionlib_msgs roscpp rospy std_msgs message_runtime
)
include_directories(
  ${catkin_INCLUDE_DIRS}
)
add_executable(DoDishes_client src/DoDishes_client.cpp)
add_executable(DoDishes_server src/DoDishes_server.cpp)
add_dependencies(DoDishes_client ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(DoDishes_server ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(DoDishes_client
  ${catkin_LIBRARIES}
)
target_link_libraries(DoDishes_server
  ${catkin_LIBRARIES}
)
catkin_install_python(PROGRAMS
  scripts/DoDishes_client.py scripts/DoDishes_server.py 
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39

(8)start_cpp.launch

<launch>
<node
    pkg="action_sample"
    type="DoDishes_client"
    name="do_dishes_client"
    required="true"
    output="screen"
/>
<node
    pkg="action_sample"
    type="DoDishes_server"
    name="do_dishes_server"
    respawn="true"
    output="screen"
/>
</launch>
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

(9)start_py.launch

<launch>
<node
    pkg="action_sample"
    type="DoDishes_client.py"
    name="do_dishes_client"
    required="true"
    output="screen"
/>
<node
    pkg="action_sample"
    type="DoDishes_server.py"
    name="do_dishes_server"
    respawn="true"
    output="screen"
/>
</launch>
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

(10)编译并运行

cd ~/catkin_ws
catkin_make --source src/action_sample 
source devel/setup.bash
roslaunch action_sample start_cpp.launch
roslaunch action_sample start_py.launch
  • 1
  • 2
  • 3
  • 4
  • 5

start_cpp运行截图:
在这里插入图片描述
start_py运行截图:
在这里插入图片描述

3 总结

本文中的例子放在了本人的github上: action_sample

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/运维做开发/article/detail/982414
推荐阅读
相关标签
  

闽ICP备14008679号