从turtlesim到贪吃蛇……

简单：蓝桥ROS机器人之极简贪吃蛇

酷炫：蓝桥ROS机器人之绚丽贪吃蛇

效果如下：

需要修改哪些功能包？

如何一步一步实现上述功能？

键盘遥控可否改成自动贪吃蛇？

部分提示如下：

import rospy

from tanksim.msg import Pose

from tanksim.srv import Spawn

from tanksim.srv import SetPen

from geometry_msgs.msg import Twist

from geometry_msgs.msg import TransformStamped

import random

import math

tank1_pose = Pose()

tanklist = []

lasttank = 1

nexttankIndex = 1

class mySpawner:

def __init__(self, tname):

self.tank_name = tname

self.state = 1

rospy.wait_for_service('/spawn')

try:

client = rospy.ServiceProxy('/spawn', Spawn)

x = random.randint(1, 10)

y = random.randint(1, 10)

theta = random.uniform(1, 3.14)

name = tname

_nm = client(x, y, theta, name)

rospy.loginfo("tank Created [%s] [%f] [%f]", name, x, y)

rospy.Subscriber(self.tank_name + '/pose', Pose, self.tank_poseCallback)

self.pub = rospy.Publisher(self.tank_name + '/cmd_vel', Twist, queue_size=10)

self.tank_to_follow = 1

self.tank_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 tank failed. %s", e)

def tank_poseCallback(self, data):

self.tank_pose = data

def tank_velocity(self, msg):

self.pub.publish(msg)

def tank1_poseCallback(data):

global tank1_pose

global lasttank

global tanklist

global nexttankIndex

tank1_pose.x = round(data.x, 4)

tank1_pose.y = round(data.y, 4)

tank1_pose.theta = round(data.theta, 4)

for i in range(len(tanklist)):

twist_data = Twist()

diff = math.sqrt(pow((tank1_pose.x - tanklist[i].tank_pose.x) , 2) + pow((tank1_pose.y - tanklist[i].tank_pose.y), 2))

ang = math.atan2(tank1_pose.y - tanklist[i].tank_pose.y, tank1_pose.x - tanklist[i].tank_pose.x) - tanklist[i].tank_pose.theta

if(ang <= -3.14) or (ang > 3.14):

ang = ang / math.pi

if (tanklist[i].state == 1):

if diff < 1.0:

tanklist[i].state = 2

tanklist[i].tank_to_follow = lasttank

lasttank = i + 2

rospy.loginfo("tank Changed [%s] [%f] [%f]", tanklist[i].tank_name, diff, ang)

nexttankIndex += 1

tanklist.append(mySpawner("tank" + str(nexttankIndex)))

else:

parPose = tank1_pose

if(tanklist[i].tank_to_follow != 1):

parPose = tanklist[tanklist[i].tank_to_follow - 2].tank_pose

diff = math.sqrt(pow((parPose.x - tanklist[i].tank_pose.x) , 2) + pow((parPose.y - tanklist[i].tank_pose.y), 2))

goal = math.atan2(parPose.y - tanklist[i].tank_pose.y, parPose.x - tanklist[i].tank_pose.x)

ang = math.atan2(math.sin(goal - tanklist[i].tank_pose.theta), math.cos(goal - tanklist[i].tank_pose.theta))

if(ang <= -3.14) or (ang > 3.14):

ang = ang / (2*math.pi)

if(diff < 0.8):

twist_data.linear.x = 0

twist_data.angular.z = 0

else:

twist_data.linear.x = 2.5 * diff

twist_data.angular.z = 20 * ang

tanklist[i].tank_velocity(twist_data)

tanklist[i].oldAngle = ang

def spawn_tank_fn():

global nexttankIndex

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

rospy.Subscriber('/tank1/pose', Pose, tank1_poseCallback)

rospy.wait_for_service("/tank1/set_pen")

try:

client = rospy.ServiceProxy('/tank1/set_pen', SetPen)

client(0,0,0,0,1)

except rospy.ServiceException as e:

print("Service call failed: %s"%e)

nexttankIndex += 1

tanklist.append(mySpawner("tank" + str(nexttankIndex)))

# for i in range(2,10):

# tanklist.append(mySpawner("tank" + str(i)))

rospy.spin()

if __name__ == "__main__":

spawn_tank_fn()

机器人

版权声明：本文内容由网络用户投稿，版权归原作者所有，本站不拥有其著作权，亦不承担相应法律责任。如果您发现本站中有涉嫌抄袭或描述失实的内容，请联系我们jiasou666@gmail.com 处理，核实后本网站将在24小时内删除侵权内容。