蓝桥ROS之半自动贪吃龟turtlesim版

为啥不是全自动？全自动是作业，半自动是提示。

一步一步完成吧，非常简单。

第一步：打开蓝桥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？

第五步：写个节点全自动？留作思考题吧 

 ^_^  ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ ^_^ 

文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。

原文链接：zhangrelay.blog.csdn.net/article/details/124503850