ROS2 控制小车进行矩形运动

需要启动的节点：

# 打开机器人底盘
ros2 launch turn_on_wheeltec_robot turn_on_wheeltec_robot.launch.py

# 2D导航
ros2 launch wheeltec_nav2 wheeltec_nav2.launch.py

查看小车当前坐标信息：

ros2 topic echo /amcl_pose

然后运行下面节点“ros2 run system_realtime_map navigation_set_node”

import rclpy  
from rclpy.node import Node  
from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped  
import math  
import time  
import threading  
from datetime import datetime, timedelta  
  
class NavigationNode(Node):  
    def __init__(self):  
        super().__init__('navigation_set_node')  
        self.start_time = datetime.now()
        # self.end_time = datetime.now() + timedelta(hours=1)
        self.out_time = 0.0
        # 发布2D导航
        self.publisher_ = self.create_publisher(PoseStamped, 'goal_pose', 10)  
        # 订阅坐标信息
        self.subscription  = self.create_subscription(PoseWithCovarianceStamped,"amcl_pose",self.pose_callback,10)
        self.subscription # 防止被垃圾回收

        # 创建一个线程来运行移动逻辑  
        threading.Thread(target=self.thread_move, args=()).start()  
  
    def pose_callback(self, msg):  
        self.start_time = datetime.now()
        print(msg.pose.pose.position)

    def setPose(self, p_x, p_y, theta_deg):  
        # 将角度从度数转换为弧度  
        theta_rad = math.radians(theta_deg)  
        goal = PoseStamped()  
        goal.header.frame_id = 'map'  
        goal.pose.position.x = p_x  
        goal.pose.position.y = p_y  
        goal.pose.position.z = 0.0  # 在2D导航中，z通常设置为0  
        # 计算四元数来表示方向  
        goal.pose.orientation.z = math.sin(theta_rad / 2) # 对于绕z轴旋转，z分量为0  
        goal.pose.orientation.w = math.cos(theta_rad / 2)  
        # 注意：对于绕z轴的简单旋转，x和y分量应该为0，这里我们明确设置为0  
        goal.pose.orientation.x = 0.0  
        goal.pose.orientation.y = 0.0  
        self.publisher_.publish(goal)  
        print(f"send {p_x},{p_y}")
        # self.get_logger().info('Sending goal: {}'.format(goal))  
  
    def thread_move(self):  
        arrX = [0.0, 0.5, 0.5, 0.0, 0.0]  
        arrY = [0.0, 0.0, 0.5, 0.5, 0.0]  
        arrTheta = [0.0, 90.0, 180.0, 270.0, 0.0]  # 使用度数  

        for i in range(len(arrX)):  
            self.setPose(arrX[i], arrY[i], arrTheta[i])  
            self.start_time = datetime.now()
            self.out_time = 0.0
            while self.out_time < 2:
                self.out_time = (datetime.now() - self.start_time).total_seconds()
                time.sleep(0.1)  # 使用秒作为单位 
                # print(f"out_time:{self.out_time}s");

  
def main(args=None):  
    rclpy.init(args=args)  
    navigation_node = NavigationNode()  
    rclpy.spin(navigation_node)  
    navigation_node.destroy_node()  
    rclpy.shutdown()  
  
if __name__ == '__main__':  
    main()