建设银行网站地址,python开发工具有哪些,杭州酒店网站设计公司推荐,做的网站能撤掉吗1、引言
在上节的驱动机器人#xff0c;我们知道是cmd_vel话题发布一串Twist类型消息来控制#xff0c;我们可以输入如下命令查看这个Twist的详细信息#xff1a;rosmsg show geometry_msgs/Twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geome…1、引言
在上节的驱动机器人我们知道是cmd_vel话题发布一串Twist类型消息来控制我们可以输入如下命令查看这个Twist的详细信息rosmsg show geometry_msgs/Twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z 由两个向量组成线速度linear和角速度angular。 然后在命令行我们对话题cmd_vel发布消息输入命令如下rostopic pub /cmd_vel geometry_msgs/Twist -- [1.0, 0.0, 0.0] [0.0, 0.0, 0.0] 这样就可以驱动机器人行驶了也就说控制差分驱动的平面运动机器人我们只需要使用上述两个参数linear就是前进与后退的速度(米/秒)angular就是绕竖直轴的角速度(弧度/秒)这个角速度被称为偏航角速度或简单地理解为机器人旋转得有多快。 这节讲解如何遥控机器人我们通过键盘来控制所以需要写一个监听键盘敲击的节点以及在keys话题上发布std_msgs/String的键盘驱动程序。
2、捕获键盘敲击
使用Python的termios和tty库将终端设置成为原始模式并捕获键盘的敲击事件然后将这些事件以std_msgs/String的形式发布出去。 我们接着在上节的工作空间里面来写cd ~/mywanderbot_ws/src/mywanderbot/src
2.1、key_publisher.py
我们来写一个keys话题通过这个话题来捕获键盘敲击然后通过std_msgs.msg.String消息类型来发布:gedit key_publisher.py
#!/usr/bin/env python
import rospy
import sys,select,tty,termios
from std_msgs.msg import Stringif __name____main__:key_pub rospy.Publisher(keys,String,queue_size1)rospy.init_node(keyboard_driver)rate rospy.Rate(100)old_attr termios.tcgetattr(sys.stdin)#print(old_attr)tty.setcbreak(sys.stdin.fileno())#print(termios.tcgetattr(sys.stdin))while not rospy.is_shutdown():if select.select([sys.stdin],[],[],0)[0] [sys.stdin]:key_pub.publish(sys.stdin.read(1))rate.sleep()termios.tcsetattr(sys.stdin,termios.TCSADRAIN,old_attr)
加一个可执行权限chmod ux key_publisher.py 2.2、获取与设置终端
这个程序使用termios库来捕获键盘敲击默认地终端会缓冲文本的一行直到用户按下回车才将这一行的文本发送到程序中。那本节我们是按下任意一个键我们就要在标准的输入流中获取它所以需要改变终端的行为。先获取做个备份然后通过setcbreak更改其为cbreak mode(中断模式),tcgetattr(fd) - list_of_attrs获取文件描述符fd的tty属性
模式更改好了之后我们就可以持续地等待标准输入流直到有字符出现虽然我们可以简单地将程序阻塞在标准输入上但是那样做将导致进程无法触发任何ROS的回调函数所以这里我们使用select函数将超时参数设置为0这样每次调用select函数就不会阻塞而是立即返回。每次循环中我们将使用rate.sleep函数消耗掉剩下的时间。 最后在程序退出之前将终端又设置成标准模式。
termios.tcsetattr(sys.stdin,termios.TCSADRAIN,old_attr)#tcsetattr解释如下
tcsetattr(fd, when, attributes) - None
为文件描述符fd设置tty属性 when确定何时更改属性 termios.TCSANOW立即改变 termios.TCSADRAIN在传送完毕后更改 termios.TCSAFLUSH传输完毕后改变队列输出并丢弃所有队列输入 2.3、select
其中select的用法如下
select(rlist, wlist, xlist[, timeout]) - (rlist, wlist, xlist)
等待直到一个或多个文件描述符准备好进行某种I/O操作 rlist -- 等待直到准备好读 wlist -- 等待直到准备好写 xlist -- 等待直到有异常 如果只需要一种条件则为其他列表设置[]即可。第四个是可选的超时参数如果不设置就永不超时。需要注意的是在Windows上只支持套接字而在Unix上既支持套接字也支持使用文件描述符。
2.4、捕获测试
我们开三个终端分别输出如下命令
roscore
cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py
rostopic echo keys
测试的时候切换到第二个终端敲击键盘我们就会在第三个终端打印keys话题信息进行显示出来。如下图 3、键盘遥控
我们通过键盘来遥控机器人看下是如何处理的通过捕获键盘字母然后如果是指定的字母我们就通过Twist消息来输出对应速度。
3.1、keys_to_twist.py
cd ~/mywanderbot_ws/src/mywanderbot/srcgedit keys_to_twist.py
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twistkey_mapping{w:[1,0],x:[-1,0],a:[0,1],d:[0,-1],s:[0,0]}def keys_cb(msg,twist_pub):if len(msg.data)0 or not key_mapping.has_key(msg.data[0]):returnvelskey_mapping[msg.data[0]]tTwist()t.linear.xvels[0]t.angular.zvels[1]twist_pub.publish(t)if __name____main__:rospy.init_node(keys_to_twist)twist_pubrospy.Publisher(cmd_vel,Twist,queue_size1)#print(twist_pub.get_num_connections)rospy.Subscriber(keys,String,keys_cb,twist_pub)rospy.spin()
这里可以看到使用key_mapping字典来获取速度通过订阅keys话题然后使用它的回调函数keys_cb在字典中查找获取的键名找到了键名就取出对应的速度最后通过cmd_vel将键盘获取的速度值发布出去即可。 加上可执行的权限chmod ux keys_to_twist.py 分别开启终端来测试下
roscore
cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py
cd ~/mywanderbot_ws/src/mywanderbot/src
python keys_to_twist.py
我们可以查看cmd_vel的显示情况在python key_publisher.py这个终端我们敲击键盘就可以看到速度向量的情况rostopic echo cmd_vel linear: x: 1.0 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 0.0 --- linear: x: 0.0 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 1.0 --- 当然也可以启动gazebo仿真机器人roslaunch turtlebot3_gazebo turtlebot3_world.launch 然后我们键盘来控制这个机器人可以看到机器人在正常运行。
3.2、keys_to_twist_rate.py
我们来看下这个发布消息的平均速率rostopic hz cmd_vel subscribed to [/cmd_vel] no new messages no new messages ... average rate: 0.198 min: 0.214s max: 51.695s std dev: 12.05897s window: 24 average rate: 0.204 min: 0.214s max: 51.695s std dev: 11.82527s window: 25 ... 我们发现每秒估计一次的平均速率的估计值基本上一直是接近0也就是仅在我们敲击键盘时才突然增长一下然后又变成了0所以为了让这个节点适用于那些需要稳定的速度命令流的机器人我们每隔0.1秒(频率为10Hz)就输出一条Twist消息为了做到这一点我们在没有收到新的命令时就重复上一次的命令。 这里我们可以在while循环里面使用sleep(0.1)来实现输出频率的控制但这样仅仅只是保证输出频率不大于10Hz而实际的运行频率很可能会由于操作系统的调度和循环本身的耗时出现较大的波动。 因为不同的计算机是有着不同的运行频率和计算性能为了保证固定不变的输出频率程序在循环中所需的实际CPU休眠时间也是不能预知的所以我们使用ROS中的rate结构来实现它会去持续地估计循环的耗时获得更一致的结果。 我们将其改进如下 cd ~/mywanderbot_ws/src/mywanderbot/srcgedit keys_to_twist_rate.py
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twistkey_mapping{w:[1,0],x:[-1,0],a:[0,1],d:[0,-1],s:[0,0]}
g_last_twistNonedef keys_cb(msg,twist_pub):global g_last_twistif len(msg.data)0 or not key_mapping.has_key(msg.data[0]):return velskey_mapping[msg.data[0]]g_last_twist.linear.xvels[0]g_last_twist.angular.zvels[1]twist_pub.publish(g_last_twist)if __name____main__:rospy.init_node(keys_to_twist_rate)twist_pubrospy.Publisher(cmd_vel,Twist,queue_size1)rospy.Subscriber(keys,String,keys_cb,twist_pub)raterospy.Rate(10)g_last_twistTwist()while not rospy.is_shutdown():twist_pub.publish(g_last_twist)rate.sleep()
加个可执行权限chmod ux keys_to_twist_rate.py
分别开启终端并执行下面命令
roscore
cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py
cd ~/mywanderbot_ws/src/mywanderbot/src
python keys_to_twist_rate.py
最后我们新开一个终端再来看下输出速率rostopic hz cmd_vel subscribed to [/cmd_vel] average rate: 10.002 min: 0.099s max: 0.101s std dev: 0.00045s window: 10 average rate: 9.998 min: 0.099s max: 0.101s std dev: 0.00050s window: 20 average rate: 9.996 min: 0.099s max: 0.101s std dev: 0.00049s window: 30 average rate: 10.000 min: 0.098s max: 0.103s std dev: 0.00081s window: 40 average rate: 10.000 min: 0.098s max: 0.103s std dev: 0.00078s window: 50 average rate: 10.000 min: 0.098s max: 0.103s std dev: 0.00079s window: 60 ... 可以看到结果基本是稳定的10Hz的输出速率。
3.3、rqt_graph
我们可以查看下目前有哪些相关节点以及它们之间的关系可以输入rostopic info cmd_vel Type: geometry_msgs/Twist Publishers: * /keys_to_twist (http://YAB:41117/) * /keys_to_twist_rate (http://YAB:40317/) Subscribers: * /rostopic_5913_1696473604053 (http://YAB:46119/) * /rostopic_5956_1696473632865 (http://YAB:45937/) * /rostopic_6042_1696476700529 (http://YAB:44365/) * /rostopic_6083_1696476719297 (http://YAB:33923/) * /rostopic_6475_1696476841619 (http://YAB:45707/) 可以看到有两个发布者下面就是很多的订阅者。
类型是geometry_msgs/Twist我们可以详细查看其结构rosmsg show geometry_msgs/Twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z 可以看到成员类型是geometry_msgs/Vector3有三个域分别是x、y、z
我们还可以对其可视化输入rqt_graph 3.4、rqt_plot
从上面我们知道了话题名称和消息中域的名字我们可以选择感兴趣的域将这些数据流绘制出来那么这节主要就是线速度X跟角速度Z所以我们对这两个域进行绘制rqt_plot cmd_vel/linear/x cmd_vel/angular/z 当我们按下键的时候速度命令就会改变我们来看下绘制的情况暂停截图如下 4、参数服务器
上面的速度绘制图我们观察发现速度一直是0、1、-1三种取值在ROS中使用的是SI(国际)单位制也就是机器人以每秒一米的速度向前向后以每秒一弧度的角速度左右旋转。 然而在不同场景机器人的运行速度是完全不一样的有的地方1m/s速度太慢了有的地方却可能太快了所以我们需要找到一种把程序参数化的方法这样它才能应用到不同的机器人。
cd ~/mywanderbot_ws/src/mywanderbot/srcgedit keys_to_twist_parameterized.py
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twistkey_mapping{w:[1,0],x:[-1,0],a:[0,1],d:[0,-1],s:[0,0]}
g_last_twistNone
g_vel_scales[0.1,0.1]def keys_cb(msg,twist_pub):global g_last_twist,g_vel_scalesif len(msg.data)0 or not key_mapping.has_key(msg.data[0]):returnvelskey_mapping[msg.data[0]]g_last_twist.linear.xvels[0]*g_vel_scales[0]g_last_twist.angular.zvels[1]*g_vel_scales[1]twist_pub.publish(g_last_twist)if __name____main__:rospy.init_node(keys_to_twist_parameterized)twist_pubrospy.Publisher(cmd_vel,Twist,queue_size1)rospy.Subscriber(keys,String,keys_cb,twist_pub)g_last_twistTwist()if rospy.has_param(~linear_scale):g_vel_scales[0]rospy.get_param(~linear_scale)else:rospy.logwarn(linear_scale not provided,using %.1f%g_vel_scales[0])if rospy.has_param(~angular_scale):g_vel_scales[1]rospy.get_param(~angular_scale)else:rospy.logwarn(angular_scale not provided,using %.1f%g_vel_scales[1])raterospy.Rate(10)while not rospy.is_shutdown():twist_pub.publish(g_last_twist)rate.sleep()加个可执行权限chmod ux keys_to_twist_parameterized.py
分别开启终端输入如下命令
roscore
cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py
cd ~/mywanderbot_ws/src/mywanderbot/src
./keys_to_twist_parameterized.py
这里如果没有指定参数就会出现警告输入不带参数 ./keys_to_twist_parameterized.py [WARN] [1696558099.873788]: linear_scale not provided,using 0.1 [WARN] [1696558099.875360]: angular_scale not provided,using 0.1 日志保存在cd ~/.ros/log这里里面的对应文件cat keys_to_twist_parameterized.log
当然日志太多用不着的话可以全部清理rospy purge
正常带参数的输入./keys_to_twist_parameterized.py _linear_scale:0.8 _angular_scale:0.7 这里的参数赋值需要注意的是使用的是海象运算符(:)
最后再开一个终端我们来测试下新的速度是否有变化rostopic echo cmd_vel 然后我们在运行key_publisher.py的这个终端按键w将出现如下速度 linear: x: 0.8 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 0.0 按键a将出现如下速度 linear: x: 0.0 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 0.7 可以看到参数的指定也更改了对应的速度值。
5、加速度
我们知道在现实世界里面任何物体都是有质量的所以机器人不能瞬间启动与停止而是需要一个加速与减速的过程而且当速度立马切换到非常快的时候开车的朋友都清楚容易出现打滑等情况所以我们也不能给机器人发送不合理的速度命令为了避免这些问题我们把速度命令设置成一个斜坡上升或下降的过程。
cd ~/mywanderbot_ws/src/mywanderbot/srcgedit keys_to_twist_ramps.py
加个可执行权限chmod ux keys_to_twist_ramps.py
我们可以先来测试下再次之前需要启动roscore然后输入./keys_to_twist_ramps.py
parameter [~linear_scale] not found,using 0.100
parameter [~angular_scale] not found,using 0.100
parameter [~linear_accel] not found,using 1.000
parameter [~angular_accel] not found,using 1.000
这段代码就是在前面的基础上做一个速度阶跃的增加与减少这样即使我们发给机器人是瞬时变化的速度或者是阶跃式的命令都会被减缓成斜坡状。主要就是ramped_vel这个函数每次被调用都会向目标速度前进一步如果距离目标速度小于一个step步长的话就直接赋值目标速度。然后通过ramped_twist函数计算得到twist再通过send_twist进行发布即可。后面的回调函数以及获取参数的方法跟前面差不多这里就不赘述了。 整体来测试下分别开启终端输入如下命令
roscore
cd ~/mywanderbot_ws/src/mywanderbot/src
python key_publisher.py
cd ~/mywanderbot_ws/src/mywanderbot/src
./keys_to_twist_ramps.py _linear_scale:0.5 _angular_scale:1.0 _linear_accel:1.0 _angular_accel:1.0
跟前面一样我们使用rqt_plot程序来生成一个实时的速度图rqt_plot cmd_vel/linear/x cmd_vel/angular/z 在key_publisher.py这个终端进行按键测试我们就能够实时看到绘制的速度图了。跟前面的速度绘制图进行比较我们也可以发现速度在上升与下降的过程都消耗了一定的时间加速度有限所以这些命令在现实中是可以实现的。 也可以直接观察仿真机器人的行驶情况打开命令roslaunch turtlebot3_gazebo turtlebot3_world.launch
6、launch文件
对于上面的节点我们每次要启动很多终端比较繁琐现在我们来回顾下以前学习过的launch文件可以批量启动节点有兴趣的可以查阅ROS机器人操作系统Catkin的编译与常用命令的使用介绍 里面有对launch文件的更具体介绍
操作如下
cd ~/mywanderbot_ws/src/mywanderbot
mkdir launch
cd launch
gedit test.launch
launch
node namekey_publisher pkgmywanderbot typekey_publisher.py respawnfalse outputscreen /
node namekeys_to_twist_ramps pkgmywanderbot typekeys_to_twist_ramps.py respawnfalse outputscreenparam namelinear_scale typedouble value0.5 /param nameangular_scale typedouble value1.0 /param namelinear_accel typedouble value1.0 /param nameangular_accel typedouble value1.0 /
/node
/launch
加个可执行权限chmod ux test.launch
执行launch文件roslaunch mywanderbot test.launch 这样就启动了定义的两个节点以及将定义的参数也会加载到参数服务器里面 我们可以查看下实时速度rqt_plot cmd_vel/linear/x cmd_vel/angular/z 如下图
