小乌龟运动控制-4两只小乌龟

ROS小乌龟系列文章目录

第一章 小乌龟划圆圈
第二章 小乌龟走方形
第三章 五角星
第四章 两只小乌龟

---

---

前言

ROS 中实现两只小乌龟一个画圆圈一个画方块的代码实现

1、生成第2个乌龟

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
import sys
import rospy
from turtlesim.srv import Spawn

def turtle_spawn():
	# ROS节点初始化
    rospy.init_node('turtle_spawn')
	# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/spawn')
    try:
        add_turtle = rospy.ServiceProxy('/spawn', Spawn) # Spwan为消息类型
		# 请求服务调用，输入请求数据
        response = add_turtle(2.0, 2.0, 0.0, "turtle2")#x,y,theta,name
        return response.name
    except rospy.ServiceException as e:
        print ("Service call failed: %s"%e)
if __name__ == "__main__":
	#服务调用并显示调用结果
    print ("Spwan turtle successfully [name:%s]" %(turtle_spawn()))

2、实现两只小乌龟一个画圆圈一个画方块

"""
<case1: 直接使用threading中的Thread类创建线程>
Date: 2024/8/15
Author: chance
"""

from threading import Thread
import time
from time import sleep
import math
import rospy
from geometry_msgs.msg import Twist

rospy.init_node('draw_circle', anonymous=True)

def draw_circle(speed):
    print("11111")
    # 初始化ROS节点
    # rospy.init_node('draw_circle', anonymous=True)
    print("2222")

    # 创建一个发布器，向/turtle1/cmd_vel话题发送Twist消息
    pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    # rate = rospy.Rate(10)  # 10Hz

    # 创建Twist消息
    vel_msg = Twist()
    # vel_msg.linear.x = 1.0  # 向前运动速度
    vel_msg.linear.x = speed  # 向前运动速度
    vel_msg.angular.z = 1.0  # 角速度


    # 发布消息
    while not rospy.is_shutdown():
        pub.publish(vel_msg)
    #     rate.sleep()
    print("11111")

# rospy.init_node('draw_square', anonymous=True)
def draw_square():
    # 初始化ROS节点
    # rospy.init_node('draw_square', anonymous=True)

    # 创建一个发布器，向/turtle2/cmd_vel话题发送Twist消息
    pub = rospy.Publisher('/turtle2/cmd_vel', Twist, queue_size=10)
    rate = rospy.Rate(1)  # 1Hz

    # 创建Twist消息
    vel_msg = Twist()

    while not rospy.is_shutdown():
        # 向前运动
        vel_msg.linear.x = 2.0
        vel_msg.angular.z = 0.0
        pub.publish(vel_msg)
        rospy.sleep(2)  # 直行2秒

        # 停止前进，准备转弯
        vel_msg.linear.x = 0.0
        pub.publish(vel_msg)
        rospy.sleep(1)

        # 左转90度
        vel_msg.angular.z = math.pi / 2  # 90度
        pub.publish(vel_msg)
        rospy.sleep(1)  # 转弯1秒

        # 停止转弯
        vel_msg.angular.z = 0.0
        pub.publish(vel_msg)
        rospy.sleep(1)


# 自定义的函数，可以替换成其他任何函数
def task(threadName, number, letter):
    print(f"【线程开始】{threadName}")
    m = 0
    while m < number:
        sleep(1)
        m += 1
        current_time = time.strftime('%H:%M:%S', time.localtime())
        print(f"[{current_time}] {threadName} 输出 {letter}")
    print(f"【线程结束】{threadName}")


thread1 = Thread(target=task, args=("thread_1", 4, "a"))  # 线程1：执行任务打印4个a
thread2 = Thread(target=draw_circle, args=(1.0,))  # 线程2：执行任务打印2个b
thread3 = Thread(target=draw_square, args=())  # 线程2：执行任务打印2个b

thread1.start()  # 线程1开始
thread2.start()  # 线程2开始
thread3.start()  # 线程3开始

thread1.join()  # 等待线程1结束
thread2.join()  # 等待线程2结束
thread3.join()  # 等待线程3结束

运行

在终端中运行ROS节点，执行以下命令：

roscore 

启动第一个小乌龟：

rosrun turtlesim turtlesim_node

启动第二个小乌龟：

rosrun turtlebot3_draw_circle draw_spawn.py 

两只小乌龟一起：

yc@yc-rl:~$ cd catkin_ws/src/turtlebot3_draw_circle/scripts/
yc@yc-rl:~/catkin_ws/src/turtlebot3_draw_circle/scripts$ python3 threading_test.py