ROS+python控制小乌龟走方形的实现rospy

常见的简陋的控制乌龟行走方形的方式很简单，例如：

代码有些地方是测试用的，可以不要。

#! /usr/bin/env pythonfrom pickle import TRUE
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from rospy.timer import TimerPI=3.141592653
turn=True
twist=Twist()
def subCallback(pose):rospy.loginfo("come into subCallback!%f",pose.x)def timeCallback(event):pub=rospy.Publisher("turtle1/cmd_vel",Twist,queue_size=1)global turnif turn:twist.linear.x=1twist.angular.z=0.0turn=Falseelse:twist.linear.x = 0twist.angular.z = 1*PI/2turn=TRUEpub.publish(twist)if __name__=="__main__":rospy.init_node("go_square")sub = rospy.Subscriber("turtle1/pose", Pose, subCallback, queue_size=10)rospy.Timer(rospy.Duration(1),timeCallback, oneshot=False)rospy.spin()pass

用rospy.Timer每隔一段固定时间改变乌龟策略，在行进和转弯中进行改变。这样的代码可能受硬件和线程影响较大，画出的方形比较粗糙。如下图：

当然，不用rospy.Timer也可以实现，但总感觉画的不踏实，比较有偶然性。

下边是模仿ROS自带的乌龟走方形的框架，利用Python写的，这样对乌龟的控制比较强，效果比较好。代码如下：

#! /usr/bin/env pythonfrom genpy.message import fill_message_args
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from rospy.timer import Timer
from math import cos, fabs, sin
from enum import EnumState=Enum('State',('MOVE','STOP_MOVE','TURN','STOP_TURN'))now_pose=Pose()
goal_pose=Pose()
g_state=State.MOVE
PI = 3.141592653
twist = Twist()
first_set_goal=Truedef subCallback(pose):global now_posenow_pose = posedef control(pub,linear,angular):global twisttwist.linear.x=lineartwist.angular.z=angularpub.publish(twist)#whether the turtle has reached the goal
def hasReachedGoal():global now_pose, goal_posereturn (fabs(now_pose.theta-goal_pose.theta)<0.001)&(fabs(now_pose.x-goal_pose.x)<0.001)&(fabs(now_pose.y-goal_pose.y)<0.001)#wheather the turtle has stopped
def hasStopped():global now_posereturn ((now_pose.linear_velocity<0.001)&(now_pose.angular_velocity<0.001))def stopMove(pub):global goal_pose,g_state,now_poseif hasStopped():g_state=State.TURNgoal_pose.x = now_pose.xgoal_pose.y = now_pose.ygoal_pose.theta = now_pose.theta+PI/2if goal_pose.theta>PI:goal_pose.theta=goal_pose.theta-2*PIelse:control(pub,0,0)rospy.loginfo('停止行走')def stopTurn(pub):global goal_pose,g_state,now_poseif hasStopped():g_state=State.MOVEgoal_pose.x = now_pose.x+cos(now_pose.theta)*2goal_pose.y = now_pose.y+sin(now_pose.theta)*2goal_pose.theta = now_pose.thetaelse:control(pub,0,0)rospy.loginfo('停止转弯')def move(pub):global g_stateif hasReachedGoal():g_state=State.STOP_MOVEcontrol(pub,0,0)else:control(pub,1,0)rospy.loginfo('直线行走move')def turn(pub):global g_stateif hasReachedGoal():g_state=State.STOP_TURNcontrol(pub,0,0)else:control(pub,0,PI/4)rospy.loginfo('转弯')def timeCallback(event):rospy.loginfo("come into timeCallback.....")pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=1)global goal_pose,now_pose, first_set_goal,g_state#if this is the first time to call this functionif first_set_goal:goal_pose.x = now_pose.x+cos(now_pose.theta)*2goal_pose.y = now_pose.y+sin(now_pose.theta)*2goal_pose.theta = now_pose.thetafirst_set_goal=Falseif g_state==State.STOP_MOVE:rospy.loginfo('状态为：'+str(g_state))stopMove(pub)elif g_state==State.MOVE:rospy.loginfo('状态为：'+str(g_state))move(pub)elif g_state==State.TURN:turn(pub)else:stopTurn(pub)if __name__ == "__main__":rospy.init_node("go_square")sub = rospy.Subscriber("turtle1/pose", Pose, subCallback,queue_size=10)rospy.Timer(rospy.Duration(0.016), timeCallback, oneshot=False)rospy.spin()pass

绘制的方形相对比较整齐。但根据计算机当前分配内存和CPU影响，有时可能仍会出现在拐弯处出现过度判断。当然，如果把：

rospy.Timer(rospy.Duration(0.016), timeCallback, oneshot=False)

中0.016改为更小数字，效果应该会更好。个人感觉应该就向微分那样，分的约细小就能判断更好。不过还未验证。
————————————————

原文为本人另一账号书写，链接：https://blog.csdn.net/jubobolv1/article/details/121693827

ROS+python控制小乌龟走方形的实现rospy相关推荐

ROS语音控制——小乌龟按设定图形路线运动
最近几天在学习语音识别.语音合成,应用到ROS当中实现一些简单的案例,下面是使用语音来控制turtle小乌龟走设定图形路线的案例. ROS小乌龟走设定图形路线(键盘控制+Python代码实现)_笨小古 ...
python控制机器人走直线_python程序控制NAO机器人行走
最近重新学习nao的官方文档,写点简单的程序回顾一下.主要是用python调用api,写下来保存着. '''Walk:small example to make nao walk''' import ...
python控制机器人走8字_爱，死亡和机器人第十四集齐马蓝中文字幕（Python处理utf8文件获取想要的内容）...
处理代码 file = "a.srt" fi = open(file, mode='r') a = fi.readline() i = 1 while len(str(a)) != ...
python小乌龟绘制迷宫_参照课本案例4.8.3，请用python实现小乌龟走迷宫的过程；...
[填空题]在 Excel 中输入文本,系统会自动进行对齐. [填空题]Excel工作表区域A2:C4中的单元格个数共有______________. [单选题]3个月婴儿,合理的喂养方式是 [单选题 ...
ROS小乌龟走设定图形路线（键盘控制+Python代码实现）
相信在很多人学习ROS的时候都会写一下这个demo,不仅是对代码能力的考察(代码语法.结构都还是相对简单的),还是对ROS话题通信这些基础概念的理解的考量. 首先命令运行 roscore rosrun ...
为什么在ROS中启动小乌龟后，无法用键盘控制？
在刚开始学习ROS系统时,通常会用小乌龟作为示例. 启动小乌龟需要输入下列三个命令.注意,每输入一个命令都需要重新打开一个终端.因为这里的每一条命令都会对终端占用. 1启动ROS核心 roscore ...
python控制小米电器以及ros通讯控制小米电器
注意事项: IP最后一位会在附近跳动 182-183 相关工具需要下载合适版本的米家,用电脑的手机模拟器,数据库软件查看获取的token 链接:https://pan.baidu.com/s/1ML ...
python 控制鼠标点击需要100ms为什么_用 Python3 和 OpenCV 替我玩一玩微信小游戏 — 跳一跳（这算外挂么）...
0 瞎弄我知道你们喜欢先看效果手残的我,始终跳不过你们这些超过 50 分的大佬.想起最近在用 Python 学习 ML (Mechine Learning, 机器学习) ,怎么用没学会,倒是里面神 ...
ros多个小乌龟_ROS小乌龟turtlesim详解
turtlesim 小乌龟模拟小乌龟的启动在安装完ROS之后,就可以启动小乌龟了,打开一个终端. 1. 首先要打开ROS服务 roscore 2. 打开一个新终端,打开小乌龟 rosrun tur ...

ROS+python控制小乌龟走方形的实现rospy

ROS+python控制小乌龟走方形的实现rospy相关推荐

最新文章

热门文章