ROS2使用Python开发动作通信

news2024/11/16 1:36:39

1.创建接口节点

cd chapt4_ws/

ros2 pkg create robot_control_interfaces --build-type ament_cmake --destination-directory src --maintainer-name "joe" --maintainer-email "1027038527@qq.com"

mkdir -p src/robot_control_interfaces/action
touch src/robot_control_interfaces/action/MoveRobot.action

# Goal: 要移动的距离
float32 distance
---
# Result: 最终的位置
float32 pose
---
# Feedback: 中间反馈的位置和状态
float32 pose
uint32 status
uint32 STATUS_MOVEING = 3
uint32 STATUS_STOP = 4

修改package.xml

<depend>rosidl_default_generators</depend>
<member_of_group>rosidl_interface_packages</member_of_group>

修改CMakeLists.txt
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveRobot.action"
)

编译节点

colcon build --packages-select robot_control_interfaces

2.创建Python Action节点

cd chapt4_ws/

ros2 pkg create example_action_rclpy --build-type ament_python --dependencies rclpy robot_control_interfaces --destination-directory src --node-name action_robot_02 --maintainer-name "joe" --maintainer-email "1027038527@qq.com"

# 手动再创建action_control_02节点文件
touch src/example_action_rclpy/example_action_rclpy/action_control_02.py

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
# 导入Action接口
from robot_control_interfaces.action import MoveRobot

class ActionControl02(Node):
"""Action客户端"""

def __init__(self, name):
super().__init__(name)
self.get_logger().info(f"节点已启动：{name}!")
self.action_client_ = ActionClient(self, MoveRobot, 'move_robot')
self.send_goal_timer_ = self.create_timer(1, self.send_goal)

def send_goal(self):
"""发送目标"""
self.send_goal_timer_.cancel()
goal_msg = MoveRobot.Goal()
goal_msg.distance = 5.0
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('Goal rejected :(')
return
self.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(f'Result: {result.pose}')

def feedback_callback(self, feedback_msg):
"""获取回调反馈"""
feedback = feedback_msg.feedback
self.get_logger().info(f'Received feedback: {feedback.pose}')

def main(args=None):
"""主函数"""
rclpy.init(args=args)
action_robot_02 = ActionControl02("action_control_02")
rclpy.spin(action_robot_02)
rclpy.shutdown()

#手动创建机器人类robot.py
touch src/example_action_rclpy/example_action_rclpy/robot.py

#!/usr/bin/env python3

import time
# 导入rclpy相关库
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from rclpy.action.server import ServerGoalHandle
# 导入接口
from robot_control_interfaces.action import MoveRobot
# 导入机器人类
from example_action_rclpy.robot import Robot
#from rclpy.executors import MultiThreadedExecutor
#from rclpy.callback_groups import MutuallyExclusiveCallbackGroup

class ActionRobot02(Node):
"""机器人端Action服务"""

def __init__(self,name):
super().__init__(name)
self.get_logger().info(f"节点已启动：{name}!")

self.robot_ = Robot()

self.action_server_ = ActionServer(
self, MoveRobot, 'move_robot', self.execute_callback
# ,callback_group=MutuallyExclusiveCallbackGroup()
)

def execute_callback(self, goal_handle: ServerGoalHandle):
"""执行回调函数,若采用默认handle_goal函数则会自动调用"""
self.get_logger().info('执行移动机器人')
feedback_msg = MoveRobot.Feedback()
self.robot_.set_goal(goal_handle.request.distance)

# rate = self.create_rate(2)
while rclpy.ok() and not self.robot_.close_goal():
# move
self.robot_.move_step()
# feedback
feedback_msg.pose = self.robot_.get_current_pose()
feedback_msg.status = self.robot_.get_status()
goal_handle.publish_feedback(feedback_msg)
# cancel check
if goal_handle.is_cancel_requested:
result = MoveRobot.Result()
result.pose = self.robot_.get_current_pose()
return result
# rate.sleep() # Rate会造成死锁，单线程执行器时不能使用
time.sleep(0.5)

goal_handle.succeed()
result = MoveRobot.Result()
result.pose = self.robot_.get_current_pose()
return result

def main(args=None):
"""主函数"""
rclpy.init(args=args)
action_robot_02 = ActionRobot02("action_robot_02")
# 采用多线程执行器解决rate死锁问题
# executor = MultiThreadedExecutor()
# executor.add_node(action_robot_02)
# executor.spin()
rclpy.spin(action_robot_02)
rclpy.shutdown()

touch src/example_action_rclpy/example_action_rclpy/robot.py

from robot_control_interfaces.action import MoveRobot
import math

class Robot():
"""机器人类，模拟一个机器人"""

def __init__(self) -> None:
self.current_pose_ = 0.0
self.target_pose_ = 0.0
self.move_distance_ = 0.0
self.status_ = MoveRobot.Feedback

def get_status(self):
"""获取状态"""
return self.status_

def get_current_pose(self):
"""获取当前位置"""
return self.current_pose_

def close_goal(self):
"""接近目标"""
return math.fabs(self.target_pose_ - self.current_pose_) < 0.01

def stop_move(self):
"""停止移动"""
self.status_ = MoveRobot.Feedback.STATUS_STOP

def move_step(self):
"""移动一小步"""
direct = self.move_distance_ / math.fabs(self.move_distance_)
step = direct * math.fabs(self.target_pose_ - self.current_pose_) * 0.1
self.current_pose_ += step # 移动一步
print(f"移动了：{step}当前位置：{self.current_pose_}")
return self.current_pose_

def set_goal(self, distance):
"""设置目标"""
self.move_distance_ = distance
self.target_pose_ += distance # 更新目标位置

if self.close_goal():
self.stop_move()
return False

self.status_ = MoveRobot.Feedback.STATUS_MOVEING # 更新状态为移动
return True

编辑package.xml

<depend>robot_control_interfaces</depend>

编辑setup.py

'action_robot_02 = example_action_rclpy.action_robot_02:main',
'action_control_02 = example_action_rclpy.action_control_02:main'

3.编译、运行节点
colcon build --packages-up-to example_action_rclpy

# 运行机器人节点
source install/setup.bash
ros2 run example_action_rclpy action_robot_02