使用 ROS 动作(Action)机制实现目标

步骤 1：创建Action文件

在ROS包的action目录下创建DetectParts.action文件：

# Goal
uint32 num_parts
---
# Result
bool success
---
# Feedback
float32 percent_complete

步骤 2：修改构建配置

在CMakeLists.txt中添加：

add_action_files(
  DIRECTORY action
  FILES DetectParts.action
)

generate_messages(
  DEPENDENCIES actionlib_msgs
)

在package.xml中添加依赖：

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>

编译工作空间以生成消息：

catkin_make

步骤 3：编写服务端代码 (server.py)

#!/usr/bin/env python

import rospy
import actionlib
from detect_parts_action.msg import DetectPartsAction, DetectPartsFeedback, DetectPartsResult

class DetectPartsServer:
    def __init__(self):
        self.server = actionlib.SimpleActionServer('detect_parts', DetectPartsAction, self.execute_cb, False)
        self.server.start()

    def execute_cb(self, goal):
        num_parts = goal.num_parts
        rate = rospy.Rate(1)  # 1Hz
        feedback = DetectPartsFeedback()
        result = DetectPartsResult()

        for i in range(1, num_parts + 1):
            if self.server.is_preempt_requested():
                rospy.loginfo("检测取消")
                self.server.set_preempted()
                return

            rospy.loginfo("检测第%d个零件", i)
            feedback.percent_complete = (i / float(num_parts)) * 100.0
            self.server.publish_feedback(feedback)
            rate.sleep()

        result.success = True
        rospy.loginfo("检测完成")
        self.server.set_succeeded(result)

if __name__ == '__main__':
    rospy.init_node('detect_parts_server')
    server = DetectPartsServer()
    rospy.spin()

步骤 4：编写客户端代码 (client.py)

#!/usr/bin/env python

import rospy
import actionlib
from detect_parts_action.msg import DetectPartsAction, DetectPartsGoal

def feedback_cb(feedback):
    percent = feedback.percent_complete
    if percent.is_integer():
        rospy.loginfo("%d%%", int(percent))
    else:
        rospy.loginfo("%.1f%%", percent)

def main():
    rospy.init_node('detect_parts_client')
    client = actionlib.SimpleActionClient('detect_parts', DetectPartsAction)
    client.wait_for_server()

    goal = DetectPartsGoal(num_parts=40)
    client.send_goal(goal, feedback_cb=feedback_cb)
    client.wait_for_result()

    if client.get_result().success:
        rospy.loginfo("检测完成")

if __name__ == '__main__':
    main()

步骤 5：运行代码

启动ROS核心：

roscore

启动服务端：

rosrun detect_parts_action server.py

启动客户端：

rosrun detect_parts_action client.py

检测第1个零件
检测第2个零件
...
检测第40个零件
检测完成

2.5%
5.0%
7.5%
...
100%
检测完成