实验目的:
自定义动作接口,要求动作客户端发送一个整型数据给服务端,动作服务端进行累加求和,在累加过程中连续反馈进度,最后反馈结果给客户端。
1、创建工作空间和自定义接口功能包
请参考:ROS2 话题通信——自定义消息类型(Python)-CSDN博客
2、自定义动作接口
在interface_pkg功能包中新建action文件夹,用于存放动作接口文件,在action目录中创建Progress.action文件
mkdir -p ./text_ws/src/interface_pkg/action
touch ./text_ws/src/interface_pkg/action/Progress.action
在Progress.action文件定义以下数据格式
int32 num
---
int32 sum
---
float64 progress
修改interface_pkg中的CMakeLists.txt文件
修改interface_pkg中的package.xml文件
<!--action构建工具依赖-->
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
保存然后编译
cd text_ws/
colcon build --packages-select interface_pkg
在install目录下面生成了我们所需要的文件,即编译成功
创建动作功能包action_pkg,新建动作服务端和动作客户端节点
cd ./text_ws/src/
ros2 pkg create action_pkg --build-type ament_python --dependencies rclpy interface_pkg --node-name action_server
touch ./action_pkg/action_pkg/action_client.py
编写动作服务端节点
import time
import rclpy #ROS2 Python接口库
from rclpy.node import Node #ROS2 节点类
from rclpy.action import ActionServer #ROS2动作服务器类
from interface_pkg.action import Progress #自定义动作接口
class ProgressActionServer(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("动作服务端!")
#创建动作服务端对象
self._action_server = ActionServer( #创建动作服务器(self、接口类型、动作名、回调函数)
self,
Progress,
'get_sum',
self.execute_callback)
def execute_callback(self,goal_handle): #执行收到动作目标之后的处理函数
#首先获取目标值
num = goal_handle.request.num
sum = 0
for i in range(1,num+1):#遍历累加,进行进度计算,并作为反馈发布
sum += i
feedback = Progress.Feedback()
feedback.progress = i/num
goal_handle.publish_feedback(feedback)
self.get_logger().info("连续反馈:%.2f" % feedback.progress)
time.sleep(1)
#响应最终结果
goal_handle.succeed()
result = Progress.Result()
result.sum = sum
self.get_logger().info("计算结果:%d" % result.sum)
return result
def main(args=None): #ROS2节点主入口main函数
rclpy.init(args=args) #ROS2 Python接口初始化
node = ProgressActionServer("action_server") #创建ROS2节点对象并进行初始化
rclpy.spin(node) #循环等待ROS2退出
node.destroy_node() #销毁节点对象
rclpy.shutdown() #关闭ROS2 Python接口
编写动作客户端节点
import rclpy #ROS2 Python接口库
from rclpy.node import Node #ROS2 节点类
from rclpy.action import ActionClient #ROS2动作客户端类
from interface_pkg.action import Progress #自定义动作接口
import sys
from rclpy.logging import get_logger
class ProgressActionClient(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("动作客户端!")
self._action_client = ActionClient(self,Progress,'get_sum')#创建动作客户端(self、接口类型、动作名)
#发送动作目标的函数
def send_goal(self,num):
#连接服务端
self._action_client.wait_for_server()
#发送请求
goal = Progress.Goal()
goal.num = num
self.future = self._action_client.send_goal_async(goal , self.feedback_callback)#(目标值,连续反馈回调函数)
self.future.add_done_callback(self.goal_response_callback)#反馈完之后的回调函数执行
#服务器收到目标之后反馈时的回调函数
def goal_response_callback(self,future):
goal_handle = future.result() #接收动作的结果
if not goal_handle.accepted: #如果动作被拒执行
self.get_logger().error('目标被拒绝!') #输出日志信息
return
self.get_logger().info('目标被接收,正在处理中') #输出日志信息,动作顺利执行
#处理最终响应结果
self.result_future = goal_handle.get_result_async() #异步获取动作最终执行的结果反馈
self.result_future.add_done_callback(self.get_result_callback)#设置一个收到动作最终结果的回调函数
#动作最终结果的回调函数
def get_result_callback(self,future):
result = future.result().result #读取动作执行的结果
self.get_logger().info('最终结果: {%d}' % result.sum) #输出日志信息结果
#连续反馈的回调函数
def feedback_callback(self,feedback_msg):
progress = feedback_msg.feedback.progress #读取反馈数据
self.get_logger().info('连续反馈数据: {%.2f}' % progress) #输出日志信息
def main(args=None): #ROS2节点主入口main函数
#动态解析传入的参数
if len(sys.argv) !=2:
get_logger("rclpy").error("请提交一个整型数据!")
rclpy.init(args=args) #ROS2 Python接口初始化
node = ProgressActionClient("action_client") #创建ROS2节点对象并进行初始化
node.send_goal(int(sys.argv[1])) #发送动作目标
rclpy.spin(node) #循环等待ROS2退出
node.destroy_node() #销毁节点对象
rclpy.shutdown() #关闭ROS2 Python接口
修改setup.py
分别开两个终端先编译一下,然后运行服务端和客户端节点
colcon build --packages-select action_pkg
source install/setup.bash
ros2 run action_pkg action_server
source install/setup.bash
ros2 run action_pkg action_client 10
测试成功。