前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >蓝桥ROS机器人之turtlesim贪吃蛇移植到Win11中

蓝桥ROS机器人之turtlesim贪吃蛇移植到Win11中

作者头像
zhangrelay
发布2022-05-01 10:59:25
2400
发布2022-05-01 10:59:25
举报

直播课遇到点小bug,没有弄好,现在已经ok。

蓝桥ROS机器人之turtlesim贪吃蛇

这里案例只是为了说明,算法不分系统,全平台支持的!

愿意花时间所有linux案例都能移植到win10/11,主要是时间太紧张了……无奈,有时候也没必要移植。

课程中截图如下:

还测试了一个movetogoal

坚持写博客,也是鼓励学生写,自己怎么能不写。

如果我做不到如何要求学生做?

要求从我第一次上课就提了,7年过去了,效果很惨淡啊,多惨淡呢?

每学期课程都会提及这个要求,但印象中,真正写的学生不到10人,但是!

但是,但是!

这10人当中,有9人是之前自己就写博客的。 

响应的学生太少了…… 

也论证了如下结果:

www.zhihu.com/question/520747377/answer/2449718139

里面重要的一句话:

我原本一直觉得师生对于课程结果的重要性是对半分的,各50%。

7年工作经历下来,教师作用说有10%都很夸张,当然也没有1%那么低。

一些群聊及主动学习或者交流,多有如下情况:


代码语言:javascript
复制
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()
本文参与 腾讯云自媒体同步曝光计划,分享自作者个人站点/博客。
原始发表:2022-04-24,如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 作者个人站点/博客 前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体同步曝光计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
相关产品与服务
云直播
云直播(Cloud Streaming Services,CSS)为您提供极速、稳定、专业的云端直播处理服务,根据业务的不同直播场景需求,云直播提供了标准直播、快直播、云导播台三种服务,分别针对大规模实时观看、超低延时直播、便捷云端导播的场景,配合腾讯云视立方·直播 SDK,为您提供一站式的音视频直播解决方案。
领券
问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档