ROS小乌龟走设定图形路线（键盘控制+Python代码实现）

相信在很多人学习ROS的时候都会写一下这个demo，不仅是对代码能力的考察（代码语法、结构都还是相对简单的），还是对ROS话题通信这些基础概念的理解的考量。

首先命令运行

roscore

rosrun turtlesim turtlesim_node

如果要通过键盘控制小乌龟的运动，则运行

rosrun turtlesim turtle_teleop_key

下面展示代码实现的小乌龟案例

1.圆形

#! /usr/bin/env python
# coding:UTF-8import turtle
import rospy
from geometry_msgs.msg import Twist # Twist数据类型包含线速度和角速度def turtle_publisher():rospy.init_node('turtle_publisher', anonymous=True)# 对节点进行初始化，命名一个叫turtle_publisher的节点turtle_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)# 实例化一个发布者对象，发布的话题名为 /turtle1/cmd_vel ,消息类型为 Twist,队列长度为10rate = rospy.Rate(10)# 设置循环的频率while not rospy.is_shutdown():# 装载消息cmd_msg = Twist()cmd_msg.linear.x = 2.0cmd_msg.angular.z = 1.0turtle_pub.publish(cmd_msg)# 发布话题消息rospy.loginfo("message have published x = %d angular z = %0.2f", cmd_msg.linear.x, cmd_msg.angular.z)# 在终端打印一份日志rate.sleep()# 按照设置的速率进行延时if __name__ == '__main__':try:turtle_publisher()except rospy.ROSInterruptException:pass

2.多种图形

#! /usr/bin/env python2
# coding:UTF-8import math
import rospy
import sys
from turtlesim.msg import Pose  # Pose数据类型包含坐标和角度
from geometry_msgs.msg import Twist  # Twist数据类型包含线速度和角速度class Turtlerun:""" 一个乌龟走图形路线的类 """def __init__(self, name):""" 初始化 """self.pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)  # 实例化发布者rospy.Subscriber("/turtle1/pose", Pose, self.turtle_run, queue_size=10)  # 实例化订阅者self.twist = Twist()self.x = None  # 乌龟目前所在x坐标self.y = None  # 乌龟目前所在y坐标self.size = 3.0  # 图形边长大小self.rate = rospy.Rate(100)  # 循环频率self.theta = None  # 乌龟初始角度self.goal = []  # 选择的图形目标点self.goals = {}  # 储存的图形目标点self.error = None  # 距离目标点的差值self.lock = 0  # 直线行走与旋转互锁self.side = 0  # 乌龟移动目标点的索引值self.aim_line = 0.035  # 乌龟转弯前走直线时距离目标距离的精度值self.aim_angle = 0.01  # 乌龟直走前转弯时距离目标角度的精度值self.i = 1self.turns = 0self.num = None  # 图形边数索引量def turtle_run(self, pose):""" 控制乌龟运动 """self.pose_get(pose)if self.i == 1:# 目标点只设定一次self.set_goal(pose)self.i += 1self.go_line()self.rorate()def pose_get(self, pose):""" 获取当前坐标和角度 """self.x = pose.xself.y = pose.yself.theta = pose.thetadef set_goal(self, pose):""" 设定目标点 """self.goals = {'squ': [[pose.x + self.size, pose.y, math.pi / 2],[pose.x + self.size, pose.y + self.size, math.pi],[pose.x, pose.y + self.size, - math.pi / 2],[pose.x, pose.y, 0]],'tri60': [[pose.x + self.size, pose.y, math.pi * 2 / 3],[pose.x + self.size / 2, pose.y + (self.size / 2 * math.tan(math.pi / 3)), - math.pi * 2 / 3],[pose.x, pose.y, 0]],'tri90': [[pose.x + self.size, pose.y, math.pi / 2],[pose.x + self.size, pose.y + self.size, -3 * math.pi / 4],[pose.x, pose.y, 0]],'hourglass': [[pose.x, pose.y, math.pi / 3],[pose.x + self.size / 2, pose.y + (self.size * math.sin(math.pi / 3)), math.pi],[pose.x - self.size / 2, pose.y + (self.size * math.sin(math.pi / 3)), - math.pi / 3],[pose.x + self.size / 2, pose.y - (self.size * math.sin(math.pi / 3)), math.pi],[pose.x - self.size / 2, pose.y - (self.size * math.sin(math.pi / 3)), math.pi / 3],[pose.x, pose.y, math.pi / 3]],}# 条件选择目标图形if name == 'squ':self.goal = self.goals['squ']elif name == 'tri60':self.goal = self.goals['tri60']elif name == 'tri90':self.goal = self.goals['tri90']elif name == 'hourglass':self.goal = self.goals['hourglass']elif name == 'circle':self.circle()self.num = len(self.goal) - 1def go_line(self):""" 走直线 """if self.lock == 0:  # 和旋转互锁# 计算现在的误差self.error = math.sqrt((self.x - self.goal[self.side][0]) ** 2 + (self.y - self.goal[self.side][1]) ** 2)if self.error > self.aim_line:  # 未到设定精度时前进self.twist.linear.x = 1.0else:self.twist.linear.x = 0self.lock = 1  # 解锁旋转self.pub.publish(self.twist)  # 发布乌龟速度def rorate(self):""" 转动角度到达另一边 """if self.lock == 1:self.error = abs(self.theta - self.goal[self.side][2])if self.error > self.aim_angle:  # 未到达转动角度精度时继续旋转self.twist.angular.z = 1.0else:self.twist.angular.z = 0self.lock = 0if self.side < self.num:self.side = self.side + 1else:self.side = 0  # 走完一轮目标点后重置目标点self.i = 1self.turns += 1print(self.turns)self.pub.publish(self.twist)  # 发布乌龟速度def circle(self):""" 走圆圈 """while not rospy.is_shutdown():self.twist.linear.x = 2.0self.twist.angular.z = 1.0self.pub.publish(self.twist)rospy.loginfo("message have published x = %d angular z = %0.2f", self.twist.linear.x, self.twist.angular.z)# 在终端打印一份日志self.rate.sleep()# 按照设置的速率进行延时if __name__ == '__main__':graph_list = ['circle', 'squ', 'tri60', 'tri90', 'hourglass']if len(sys.argv) > 1 and sys.argv[1] in graph_list:  # 实现rosrun xx yy.py squ直接跑对应图形name = sys.argv[1]else:name = input('Please input graph name(circle squ tri60 tri90 hourglass): ')  # 没在命令输对图形名称时提示输入try:rospy.init_node('turtle_run', anonymous=True)  # 对节点进行初始化，命名一个叫turtle_run的节点name = str(name)tgraph = Turtlerun(name)rospy.spin()except rospy.ROSInterruptException:pass

下面是走方形的案例实现：

ROS小乌龟走设定图形路线（键盘控制+Python代码实现）相关推荐

ROS语音控制——小乌龟按设定图形路线运动
最近几天在学习语音识别.语音合成,应用到ROS当中实现一些简单的案例,下面是使用语音来控制turtle小乌龟走设定图形路线的案例. ROS小乌龟走设定图形路线(键盘控制+Python代码实现)_笨小古 ...
为什么在ROS中启动小乌龟后，无法用键盘控制？
在刚开始学习ROS系统时,通常会用小乌龟作为示例. 启动小乌龟需要输入下列三个命令.注意,每输入一个命令都需要重新打开一个终端.因为这里的每一条命令都会对终端占用. 1启动ROS核心 roscore ...
基于LQR的倒立摆控制——python代码——dlqr步骤推导
推荐一个自动控制小车开源项目:本文结合老王自动驾驶控制算法第五讲的离散LQR进行学习复盘 Inverted Pendulum Control - PythonRobotics documentatio ...
ROS探索总结（七）（八）（九）——smartcar源码上传键盘控制操作杆控制
ROS探索总结(七)--smartcar源码上传看到前面写的博客还是帮助了很多ROS的学习者,我感到非常荣幸.其实我也是一名ROS的新手,ROS的相关资料少,上手难度大,我现在也在摸索着学习,还希望 ...
ros----键盘控制机器人（2）【键盘控制文件书写】
一.前言在上一篇我们着重写了键盘控制的整体思路和launch文件的书写,这一篇,我就着重写一下我从网上找到的键盘控制cpp代码和python代码. let's go!!!!!! 二.代码集合 1.c ...
ROSNOTE ：键盘控制机器人运动
ROS与底盘的通信协议: ROS平台与小车底盘通信一般是通过串口写入串口的内容是左右轮的速度从串口中读取到的是小车x,y坐标,方向角,角速度 ROS平台串口处理程序: 主要是写在base_con ...
ROS+python控制小乌龟走方形的实现rospy
常见的简陋的控制乌龟行走方形的方式很简单,例如: 代码有些地方是测试用的,可以不要. #! /usr/bin/env pythonfrom pickle import TRUE import rosp ...
多车调度问题(大疆Robot Master)——ROS键盘控制失灵，小车无法收敛定位，路径规划出错
问题1 ROS键盘控制小车失灵具体就是:用键盘左右转小车,速度贼快,而且方向不正确,检查发现是控制模块失灵,有可能是内部测量元件(陀螺仪等)烧了,换了个控制模块解决. 问题2 小车无法收敛定位具体 ...
[转]基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动
原文出处: https://blog.csdn.net/Forrest_Z/article/details/55002484 准备工作 1.下载串口通信的ROS包 (1)cd ~/catkin_ws/ ...

ROS小乌龟走设定图形路线（键盘控制+Python代码实现）

ROS小乌龟走设定图形路线（键盘控制+Python代码实现）相关推荐

最新文章

热门文章