贪吃蛇复现-CoCube

Posted 2022-12-16 zhangrelay

需要完成下面所提及博文中里面所有前序案例： 

从开环到闭环的旅程-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 或关闭并打开标记显示（不要问为什么会这样）

---