需要完成下面所提及博文中里面所有前序案例：
从开环到闭环的旅程-CoCube
在完成如上代码之后，添加一个彩蛋，贪吃蛇的案例。
蓝桥ROS之半自动贪吃龟turtlesim版
基本上就是上述代码复现一下，完全没有难度的。
贪吃蛇复现-CoCube
python代码如下：
import rospy
from turtlesim.msg import Pose
from turtlesim.srv import Spawn
from turtlesim.srv import SetPen
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TransformStamped
import random
import math
turtle1_pose = Pose()
turtlelist = []
lastturtle = 1
nextturtleIndex = 1
class mySpawner:
def __init__(self, tname):
self.turtle_name = tname
self.state = 1
rospy.wait_for_service('/spawn')
try:
client = rospy.ServiceProxy('/spawn', Spawn)
x = random.randint(1, 18)
y = random.randint(1, 18)
theta = random.uniform(1, 3.14)
name = tname
_nm = client(x, y, theta, name)
rospy.loginfo("turtle Created [%s] [%f] [%f]", name, x, y)
rospy.Subscriber(self.turtle_name + '/pose', Pose, self.turtle_poseCallback)
self.pub = rospy.Publisher(self.turtle_name + '/cmd_vel', Twist, queue_size=10)
self.turtle_to_follow = 1
self.turtle_pose = Pose()
rospy.wait_for_service("/" + tname + '/set_pen')
try:
client = rospy.ServiceProxy("/" + tname + '/set_pen', SetPen)
client(0,0,0,0,1)
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
except rospy.ServiceException as e:
print("Service tp spawn a turtle failed. %s", e)
def turtle_poseCallback(self, data):
self.turtle_pose = data
def turtle_velocity(self, msg):
self.pub.publish(msg)
def turtle1_poseCallback(data):
global turtle1_pose
global lastturtle
global turtlelist
global nextturtleIndex
turtle1_pose.x = round(data.x, 4)
turtle1_pose.y = round(data.y, 4)
turtle1_pose.theta = round(data.theta, 4)
for i in range(len(turtlelist)):
twist_data = Twist()
diff = math.sqrt(pow((turtle1_pose.x - turtlelist[i].turtle_pose.x) , 2) + pow((turtle1_pose.y - turtlelist[i].turtle_pose.y), 2))
ang = math.atan2(turtle1_pose.y - turtlelist[i].turtle_pose.y, turtle1_pose.x - turtlelist[i].turtle_pose.x) - turtlelist[i].turtle_pose.theta
if(ang <= -3.14) or (ang > 3.14):
ang = ang / math.pi
if (turtlelist[i].state == 1):
if diff < 1.0:
turtlelist[i].state = 2
turtlelist[i].turtle_to_follow = lastturtle
lastturtle = i + 2
rospy.loginfo("turtle Changed [%s] [%f] [%f]", turtlelist[i].turtle_name, diff, ang)
nextturtleIndex += 1
turtlelist.append(mySpawner("turtle" + str(nextturtleIndex)))
else:
parPose = turtle1_pose
if(turtlelist[i].turtle_to_follow != 1):
parPose = turtlelist[turtlelist[i].turtle_to_follow - 2].turtle_pose
diff = math.sqrt(pow((parPose.x - turtlelist[i].turtle_pose.x) , 2) + pow((parPose.y - turtlelist[i].turtle_pose.y), 2))
goal = math.atan2(parPose.y - turtlelist[i].turtle_pose.y, parPose.x - turtlelist[i].turtle_pose.x)
ang = math.atan2(math.sin(goal - turtlelist[i].turtle_pose.theta), math.cos(goal - turtlelist[i].turtle_pose.theta))
if(ang <= -3.14) or (ang > 3.14):
ang = ang / (2*math.pi)
if(diff < 1.0):
twist_data.linear.x = 0
twist_data.angular.z = 0
else:
twist_data.linear.x = 2.5 * diff
twist_data.angular.z = 20 * ang
turtlelist[i].turtle_velocity(twist_data)
turtlelist[i].oldAngle = ang
def spawn_turtle_fn():
global nextturtleIndex
rospy.init_node('snake_turtle', anonymous=True)
rospy.Subscriber('/turtle1/pose', Pose, turtle1_poseCallback)
rospy.wait_for_service("/turtle1/set_pen")
try:
client = rospy.ServiceProxy('/turtle1/set_pen', SetPen)
client(0,0,0,0,1)
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
nextturtleIndex += 1
turtlelist.append(mySpawner("turtle" + str(nextturtleIndex)))
# for i in range(2,10):
# turtlelist.append(mySpawner("turtle" + str(i)))
rospy.spin()
if __name__ == "__main__":
spawn_turtle_fn()随机在x,y:1～18坐标位置， 生成CoCube。
client = rospy.ServiceProxy('/spawn', Spawn)
x = random.randint(1, 18)
y = random.randint(1, 18)
theta = random.uniform(1, 3.14)
name = tname
_nm = client(x, y, theta, name)前后乌龟保持距离可以参考：
if(ang <= -3.14) or (ang > 3.14):
ang = ang / (2*math.pi)
if(diff < 1.0):
twist_data.linear.x = 0
twist_data.angular.z = 0
else:
twist_data.linear.x = 2.5 * diff
twist_data.angular.z = 20 * ang通过：
if(diff < 1.0):设置距离。
当领头的CoCube跑特别快时候，需要调整：
twist_data.linear.x = 2.5 * diff
twist_data.angular.z = 20 * ang否则，会追不上的。
前序案例：
差动驱动机器人轨迹-CoCube
迷宫逃离的问题-CoCube
自由运动和环境限制-CoCube
三维环境贪吃蛇后续补充，可以参考：
ROS2与Rviz2的贪吃蛇代码学习
主要将其中方块替换为CoCube三维模型。
rviz2中的蛇游戏；这是为了好玩，作为 ROS2 的介绍。
要求
目前，这仅rclcpp针对 ROS2 Galactic/Humble 进行了测试，尽管它很可能在任何稍旧的设备上都可以正常工作。此外，您需要安装 rviz2 和 ncurses（用于用户输入），通过sudo apt-get install libncurses-dev.运行
首先，配置 ros2 环境。通过进入根目录并运行colcon build.
源项目通过source install/setup.bash.
通过运行游戏ros2 run snake_publisher publisher。
在单独的终端中，运行rviz2 src/snake_publisher/rvizSetup.rviz.
这样，游戏就应该运行了。输入是在ros2 run运行的终端上进行的。配置
目前实现了以下节点参数：game_fps- 游戏更新的速率。
input_fps- 从队列中处理输入的速率。
snake_color_*- 蛇在其 RGB 组件中的颜色。
fruit_color_*- RGB 成分中水果的颜色。
grid_color_*- 网格在其 RGB 组件中的颜色。限制/错误
没有提供启动文件（将来可以制作一个）
如果足够好来填满整个棋盘，游戏就会崩溃。
一段时间后，消息会慢慢延迟明显的数量；只需重新启动 rviz2 或关闭并打开标记显示（不要问为什么会这样）
