深圳华宫建设集团网站,旺店通erp系统,wordpress301改不回来,网站做服务端这些应用功能的实现中#xff0c;另外一种ROS通信机制也会被常常用到——那就是动作。从这个名字上就可以很好理解这个概念的含义#xff0c;这种通信机制的目的就是便于对机器人某一完整行为的流程进行管理。 回到顶部
一、通信模型
举个例子#xff0c;比如我们想让机器…这些应用功能的实现中另外一种ROS通信机制也会被常常用到——那就是动作。从这个名字上就可以很好理解这个概念的含义这种通信机制的目的就是便于对机器人某一完整行为的流程进行管理。回到顶部一、通信模型举个例子比如我们想让机器人转个圈这肯定不是一下就可以完成的机器人得一点一点旋转直到360度才能结束假设机器人并不在我们眼前发出指令后我们根本不知道机器人到底有没有开始转圈转到哪里了image-20220528005012082OK现在我们需要的是一个反馈比如每隔1s告诉我们当前转到多少度了10度、20度、30度一段时间之后到了360度再发送一个信息表示动作执行完成。这样一个需要执行一段时间的行为使用动作的通信机制就更为合适就像装了一个进度条我们可以随时把控进度如果运动过程当中我们还可以随时发送一个取消运动的命令。1.1 客户端/服务器模型动作和服务类似使用的也是客户端和服务器模型客户端发送动作的目标想让机器人干什么服务器端执行动作过程 控制机器人达到运动的目标同时周期反馈动作执行过程中的状态。image8客户端发送一个运动的目标想让机器人动起来服务器端收到之后就开始控制机器人运动一边运动一边反馈当前的状态如果是一个导航动作这个反馈可能是当前所处的坐标如果是机械臂抓取这个反馈可能又是机械臂的实时姿态当运动执行结束后服务器再反馈一个动作结束的信息整个通信过程就此结束。1.2 一对多通信和服务一样动作通信中的客户端可以有多个大家都可以发送运动命令但是服务器端只能有一个毕竟只有一个机器人先执行完成一个动作才能执行下一个动作。1.3 同步通信既然有反馈那动作也是一种同步通信机制之前我们也介绍过动作过程中的数据通信接口使用.action文件进行定义。1.4 由服务和话题合成我们再仔细看下上边的动图是不是还会发现一个隐藏的秘密。动作的三个通信模块竟然有两个是服务一个是话题当客户端发送运动目标时使用的是服务的请求调用服务器端也会反馈一个应带表示收到命令动作的反馈过程其实就是一个话题的周期发布服务器端是发布者客户端是订阅者。没错动作是一种应用层的通信机制其底层就是基于话题和服务来实现的。回到顶部二、动作案例2.1 小海龟的动作我们使用小海龟的案例加深对动作概念的理解。进入桌面系统启动第一个终端运行小海龟仿真器piNanoPC-T6:~/dev_ws$ ros2 run turtlesim turtlesim_node该指令将启动一个蓝色背景的海龟仿真器启动第二个终端运行如下指令piNanoPC-T6:~/dev_ws$ ros2 run turtlesim turtle_teleop_key该指令将启动一个键盘控制节点在该终端中点击键盘上的“上下左右”按键就可以控制小海龟运动啦。接下来使用action命令控制小海龟的动作可以让海龟运动到某一指定的姿态piNanoPC-T6:~/Desktop$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute {theta: -1.57}Waiting for an action server to become available...Sending goal:theta: -1.57Goal accepted with ID: 08d36694d66f4977b1bbf0f2bdd93d69Result:delta: 1.5360000133514404Goal finished with status: SUCCEEDED如果需要反馈结果可以执行piNanoPC-T6:~/Desktop$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute {theta: 0} --feedbackWaiting for an action server to become available...Sending goal:theta: 0.0Goal accepted with ID: 2bbb1d8a0a7d474a9b5a41c2c2f0cd03Feedback:remaining: 1.5520000457763672Feedback:remaining: 1.5360000133514404Feedback:remaining: 1.5199999809265137Feedback:remaining: 1.503999948501587......2.2 机器人画圆如何通过代码来实现动作的编程呢动作虽然是基于话题和服务实现的但在实际使用中并不会直接使用话题和服务的编程方法而是有一套针对动作特性封装好的编程接口接下来我们就来看看 动作到底该如何实现。image-20220528010032315假设我们有一个机器人我们希望通过动作的通信方法让机器人转个圈请编程实现动作通信中客户端和服务器端的实现过程。我们首先创建my_learning_action的Python版本的功能包piNanoPC-T6:~/dev_ws$ cd srcpiNanoPC-T6:~/dev_ws/src$ ros2 pkg create --build-type ament_python my_learning_action修改功能包my_learning_action依赖package.xmldependmy_learning_interface/depend2.2.1 接口定义我们使用的动作并不是ROS中的标准定义我们通过MoveCircle.action进行自定义。我们在my_learning_interface文件夹下创建子文件夹action接着新建文件MoveCircle.actionbool enable # 定义动作的目标表示动作开始的指令---bool finish # 定义动作的结果表示是否成功执行---int32 state # 定义动作的反馈表示当前执行到的位置动作由三个部分组成第一块是动作的目标enable为true时表示开始运动第二块是动作的执行结果finish为true表示动作执行完成第三块是动作的周期反馈表示当前机器人旋转到的角度。完成定义后还需要在功能包的CMakeLists.txt中配置编译选项让编译器在编译过程中根据接口定义自动生成不同语言的代码...find_package(rosidl_default_generators REQUIRED)rosidl_generate_interfaces(${PROJECT_NAME}action/MoveCircle.action)...编译程序piNanoPC-T6:~/dev_ws$ colcon build --paths src/my_learning_interface2.2.2 通信模型通信模型如下图所示客户端发送给一个动作目标服务器控制机器人开始运动并周期反馈结束后反馈结束信息。image-20220528010217043思路理清楚接下来开始写代码。相比之前话题和服务的程序动作通信的例程相对较长我们一起来运行并分析一下。2.2.3 服务端打开my_learning_action功能包在my_learning_action文件夹下创建action_move_server.pyROS2动作示例-负责执行圆周运动动作的服务端author: zysince : 2025/12/14import timeimport rclpy # ROS2 Python接口库from rclpy.node import Node # ROS2 节点类from rclpy.action import ActionServer # ROS2 动作服务器类from my_learning_interface.action import MoveCircle # 自定义的圆周运动接口class MoveCircleActionServer(Node):def __init__(self, name):super().__init__(name) # ROS2节点父类初始化self._action_server ActionServer( # 创建动作服务器接口类型、动作名、回调函数self,MoveCircle,move_circle,self.execute_callback)def execute_callback(self, goal_handle): # 执行收到动作目标之后的处理函数self.get_logger().info(Moving circle...)feedback_msg MoveCircle.Feedback() # 创建一个动作反馈信息的消息for i in range(0, 360, 30): # 从0到360度执行圆周运动并周期反馈信息feedback_msg.state i # 创建反馈信息表示当前执行到的角度self.get_logger().info(Publishing feedback: %d % feedback_msg.state)goal_handle.publish_feedback(feedback_msg) # 发布反馈信息time.sleep(0.5)goal_handle.succeed() # 动作执行成功result MoveCircle.Result() # 创建结果消息result.finish Truereturn result # 反馈最终动作执行的结果def main(argsNone): # ROS2节点主入口main函数rclpy.init(argsargs) # ROS2 Python接口初始化node MoveCircleActionServer(action_move_server) # 创建ROS2节点对象并进行初始化rclpy.spin(node) # 循环等待ROS2退出node.destroy_node() # 销毁节点对象rclpy.shutdown() # 关闭ROS2 Python接口完成代码的编写后需要设置功能包的编译选项让系统知道Python程序的入口打开功能包的setup.py文件加入如下入口点的配置entry_points{console_scripts: [action_move_server my_learning_action.action_move_server:main,],},2.2.4 客户端在my_learning_action文件夹下创建action_move_client.pyROS2动作示例-请求执行圆周运动动作的客户端author: zysince : 2025/12/14import rclpy # ROS2 Python接口库from rclpy.node import Node # ROS2 节点类from rclpy.action import ActionClient # ROS2 动作客户端类from my_learning_interface.action import MoveCircle # 自定义的圆周运动接口class MoveCircleActionClient(Node):def __init__(self, name):super().__init__(name) # ROS2节点父类初始化self._action_client ActionClient( # 创建动作客户端接口类型、动作名self, MoveCircle, move_circle)def send_goal(self, enable): # 创建一个发送动作目标的函数goal_msg MoveCircle.Goal() # 创建一个动作目标的消息goal_msg.enable enable # 设置动作目标为使能希望机器人开始运动self._action_client.wait_for_server() # 等待动作的服务器端启动self._send_goal_future self._action_client.send_goal_async( # 异步方式发送动作的目标goal_msg, # 动作目标feedback_callbackself.feedback_callback) # 处理周期反馈消息的回调函数self._send_goal_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().info(Goal rejected :()returnself.get_logger().info(Goal accepted :)) # 动作被顺利执行self._get_result_future goal_handle.get_result_async() # 异步获取动作最终执行的结果反馈self._get_result_future.add_done_callback(self.get_result_callback) # 设置一个收到最终结果的回调函数def get_result_callback(self, future): # 创建一个收到最终结果的回调函数result future.result().result # 读取动作执行的结果self.get_logger().info(Result: {%d} % result.finish) # 日志输出执行结果def feedback_callback(self, feedback_msg): # 创建处理周期反馈消息的回调函数feedback feedback_msg.feedback # 读取反馈的数据self.get_logger().info(Received feedback: {%d} % feedback.state)def main(argsNone): # ROS2节点主入口main函数rclpy.init(argsargs) # ROS2 Python接口初始化node MoveCircleActionClient(action_move_client) # 创建ROS2节点对象并进行初始化node.send_goal(True) # 发送动作目标rclpy.spin(node) # 循环等待ROS2退出node.destroy_node() # 销毁节点对象rclpy.shutdown() # 关闭ROS2 Python接口完成代码的编写后需要设置功能包的编译选项让系统知道Python程序的入口打开功能包的setup.py文件加入如下入口点的配置entry_points{console_scripts: [action_move_client my_learning_action.action_move_client:main,action_move_server my_learning_action.action_move_server:main,],},2.2.5 编译运行编译程序piNanoPC-T6:~/dev_ws$ colcon build --paths src/my_learning_action启动第一个终端启动动作示例的服务端piNanoPC-T6:~/dev_ws$ ros2 run my_learning_action action_move_server[INFO] [1765726344.703410263] [action_move_server]: Moving circle...[INFO] [1765726344.704012816] [action_move_server]: Publishing feedback: 0[INFO] [1765726345.207813520] [action_move_server]: Publishing feedback: 30[INFO] [1765726345.712013457] [action_move_server]: Publishing feedback: 60[INFO] [1765726346.216313390] [action_move_server]: Publishing feedback: 90[INFO] [1765726346.720621950] [action_move_server]: Publishing feedback: 120[INFO] [1765726347.223664991] [action_move_server]: Publishing feedback: 150[INFO] [1765726347.727581949] [action_move_server]: Publishing feedback: 180[INFO] [1765726348.231748272] [action_move_server]: Publishing feedback: 210[INFO] [1765726348.735497200] [action_move_server]: Publishing feedback: 240[INFO] [1765726349.239607404] [action_move_server]: Publishing feedback: 270[INFO] [1765726349.743132014] [action_move_server]: Publishing feedback: 300[INFO] [1765726350.247313968] [action_move_server]: Publishing feedback: 330启动第二个终端启动动作示例的客户端piNanoPC-T6:~/dev_ws$ ros2 run my_learning_action action_move_client[INFO] [1765726344.695723183] [action_move_client]: Goal accepted :)[INFO] [1765726344.705272754] [action_move_client]: Received feedback: {0}[INFO] [1765726345.212856772] [action_move_client]: Received feedback: {30}[INFO] [1765726345.716884342] [action_move_client]: Received feedback: {60}[INFO] [1765726346.221326019] [action_move_client]: Received feedback: {90}[INFO] [1765726346.725441505] [action_move_client]: Received feedback: {120}[INFO] [1765726347.227005493] [action_move_client]: Received feedback: {150}[INFO] [1765726347.732291176] [action_move_client]: Received feedback: {180}[INFO] [1765726348.236114807] [action_move_client]: Received feedback: {210}[INFO] [1765726348.740333462] [action_move_client]: Received feedback: {240}[INFO] [1765726349.244567077] [action_move_client]: Received feedback: {270}[INFO] [1765726349.747922695] [action_move_client]: Received feedback: {300}[INFO] [1765726350.251839829] [action_move_client]: Received feedback: {330}[INFO] [1765726350.757741242] [action_move_client]: Result: {1}终端中我们可以看到客户端发送动作目标之后服务器端开始模拟机器人运动每30度发送一次反馈信息最终完成运动并反馈结束运动的信