You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
43 lines
1.2 KiB
43 lines
1.2 KiB
import rclpy
|
|
from rclpy.node import Node
|
|
from nav2_msgs.action import NavigateToPose
|
|
from rclpy.action import ActionClient
|
|
from geometry_msgs.msg import PoseStamped
|
|
import time
|
|
|
|
class MultiNav(Node):
|
|
def __init__(self):
|
|
super().__init__('multi_nav')
|
|
self.client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
|
|
self.goals = [
|
|
[1.0, 0.0, 0.0],
|
|
[1.0, 1.0, 1.57],
|
|
[0.0, 1.0, 3.14],
|
|
[0.0, 0.0, -1.57]
|
|
]
|
|
|
|
def send_goal(self, x, y, yaw):
|
|
goal_msg = NavigateToPose.Goal()
|
|
goal_msg.pose.header.frame_id = 'map'
|
|
goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
|
|
goal_msg.pose.pose.position.x = x
|
|
goal_msg.pose.pose.position.y = y
|
|
goal_msg.pose.pose.orientation.z = yaw
|
|
self.client.wait_for_server()
|
|
self.client.send_goal_async(goal_msg)
|
|
|
|
def run(self):
|
|
for x, y, yaw in self.goals:
|
|
self.send_goal(x, y, yaw)
|
|
time.sleep(10) # 等待导航完成，可改为监听反馈
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
node = MultiNav()
|
|
node.run()
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
if __name__ == '__main__':
|
|
main()
|