程序如下:

#!/usr/bin/env python
import rospy

from geometry_msgs.msg import Twist
import math

if __name__ == '__main__':
    try:
        # 初始化ROS节点

        rospy.init_node('turtle_square_movement', anonymous=True)
        # 创建发布者,发布速度指令
        velocity_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

        # 定义线速度
        linear_speed = 0.5
        # 定义正方形边长
        side_length = 5
        # 计算移动所需时间
        move_time = side_length / linear_speed
        # 定义旋转角度(90度,转换为弧度)
        rotate_angle = math.pi / 2
        # 定义旋转角速度
        angular_speed = 1.0
        # 计算旋转所需时间
        rotate_time = rotate_angle / angular_speed

        # 循环四次,完成正方形的四条边
        for _ in range(4):
            # 创建速度消息对象
            vel_msg = Twist()
            # 设置线速度
            vel_msg.linear.x = linear_speed
            # 记录开始时间
            start_time = rospy.Time.now().to_sec()
            while (rospy.Time.now().to_sec() - start_time) < move_time:
                # 发布速度消息
                velocity_pub.publish(vel_msg)


            # 停止移动
            vel_msg.linear.x = 0
            velocity_pub.publish(vel_msg)


            # 设置角速度
            vel_msg.angular.z = angular_speed
            start_time = rospy.Time.now().to_sec()
            while (rospy.Time.now().to_sec() - start_time) < rotate_time:
                # 发布速度消息
                velocity_pub.publish(vel_msg)

            # 停止旋转
            vel_msg.angular.z = 0
            velocity_pub.publish(vel_msg)

        # 最终停止乌龟
        stop_msg = Twist()
        velocity_pub.publish(stop_msg)

    except rospy.ROSInterruptException:
        pass

效果如上。

Logo

技术共进,成长同行——讯飞AI开发者社区

更多推荐