1、動作通訊簡介
機器人是一個複雜的智能系統,並不僅僅是鍵盤遙控運動、識別某個目標這麼簡單,我們需要實現的是送餐、送貨、分揀等滿足具體場景需求的機器人。在這些應用功能的實現中,另外一種ROS通信機制也會被常常用到——那就是動作。
從這個名字上就可以很好理解這個概念的含義,這種通信機制的目的就是便於對機器人某一完整行爲的流程進行管理。
動作通訊是一種帶有連續反饋的通訊模型,在通訊雙方中,客戶端發送請求數據到服務端,服務端響應結果給客戶端,但是在服務端接收到請求產生最終響應的過程中,會發送連續的反饋信息給客戶端。
客戶端發送一個運動的目標,想讓機器人動起來,服務器端收到之後,就開始控制機器人運動,一邊運動,一邊反饋當前的狀態。
如果是一個導航動作,這個反饋可能是當前所處的座標,如果是機械臂抓取,這個反饋可能又是機械臂的實時姿態。
動作通訊客戶端/服務器模型如下:
功能如下:動作客戶端提交一個整數類型num,動作服務器接收請求數據並累加1-num之間的所有整數,將最終結果返回給客戶端,且每次累計計算後將當前結果返回給客戶端
2、新建功能包
2.1、動作通訊需要先創建通訊接口
cd ~/ros_ws/src ros2 pkg create --build-type ament_cmake pkg_interfaces
2.2、接下來在pkg_interfaces功能包下創建一個action文件夾,並在action文件夾內新建【Progress.action】文件,內容如下:
int64 num
---
int64 sum
---
float64 progress
2.3、在package.xml中添加依賴包,內容如下:
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
2.4、在CMakeLists.txt中添加如下配置:
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/Progress.action"
2.5、編譯功能包
cd ~/ros_ws colcon build --packages-select pkg_interfaces
編譯完成後在工作空間的install目錄下將生成Progress.action文件對應的C++和Python文件,可以在終端進入工作空間,通過如下命令查看是否編譯成功:
source install/setup.bash
ros2 interface show pkg_interfaces/action/Progress
如果終端輸出與Progress.action文件一致的內容則正常。
3、創建動作功能包
cd ~/ros_ws/src
ros2 pkg create pkg_action --build_type ament_python --dependencies rclpy pkg_interfaces --node-name action_server_demo
4、服務端實現
import time
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from pkg_interfaces.action import Progress
class Action_Server(Node):
def __init__(self):
super().__init__('progress_action_server')
# 創建動作服務端
self._action_server = Action_Server(
self, Progress, 'get_sum', self.execute_callback
)
self.get_logger().info("action service starting!!!")
def execute_callback(self, goal_handle):
self.get_logger().info('start execute task...')
# 生成連續反饋
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('callback: %.2f' % feedback_msg.progress)
goal_handle.publish_feedback(feedback_msg)
time.sleep(1)
# 生成最終響應
goal_handle.succeed()
result = Progress.Result()
result.sum = sum
self.get_logger().info("task done!!!")
return result
def main(args=None):
rclpy.init(args=args)
Progress_action_server = Action_Server()
rclpy.spin(Progress_action_server)
Progress_action_server.destroy_node()
rclpy.shutdown()
5、客戶端實現
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from pkg_interfaces.action import Progress
class Action_Client(Node):
def __init__(self):
super().__init__('progress_action_client')
# 創建動作客戶端
self._action_client = Action_Client(self, Progress, 'get_sum')
def send_goal(self, num):
# 發送請求
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):
# 處理目標發送後的反饋
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('request refuesd')
return
self.get_logger().info('request received, start execute task')
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('final result sum = %d' % result.sum)
rclpy.shutdown
# 處理連續返回
def feedback_callback(self, feedback_msg):
feedback = (int)(feedback_msg.feedback.progress * 100)
self.get_logger().info('now progress: %d%%' % feedback)
def main(args=None):
rclpy.init(args=args)
action_client = Action_Client()
action_client.send_goal(10)
rclpy.spin(action_client)
6、編輯配置文件、編譯工作空間
cd ~/ros_ws
colcon build --packages-select pkg_action source install/setup.bash
7、運行程序
分終端分別啓動服務端和客戶端
ros2 run pkg_action action_server_demo
ros2 run pkg_action action_client_demo