为啥不是全自动？全自动是作业，半自动是提示。
?
?
一步一步完成吧，非常简单。
第一步：打开蓝桥ROS
www.lanqiao.cn/courses/854
第二步：双击xfce终端，分别在不同窗口开启roscore和turtlesim
?
?
第三步：贪吃龟（蓝桥ROS机器人之turtlesim贪吃蛇）?
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()
?
新开窗口输入python snake.py
第四步：输入坐标实现贪吃龟（ROS机器人从起点到终点（四）蓝桥云实践复现）
一个bug点！符号-0.1到0.1跳变，3.14？ -3.14？
?
?
第五步：写个节点全自动？留作思考题吧?
?^_^??^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?^_^?
?
