编写ROS节点
1.ROS节点概述
simple_mover 只发布关节角度命令 simple_arm 。
在了解用Python编写的ROS节点的一般结构之后,现在要编写另一个名为 arm_mover 的节点。
arm_mover 提供一种叫做 safe_move 的服务,它允许机械臂移动到其工作区内被认为是“安全”的任何位置。安全区域由最小和最大关节角度限制,可通过ROS参数服务器进行配置。
最后一个节点是 look_away 节点。此节点订阅正在发布摄像机数据的主题。当相机检测到具有均匀颜色的图像时,意味着它正在观察天空,节点将调用 safe_move 服务将手臂移动到新位置。
2.ROS发布者
发布者允许节点向主题发送消息,以便来自节点的数据可以用在ROS系统的其他部分中。在Python中,ROS发布者通常具有以下定义格式,尽管其他参数和参数也是可能的:
pub1 = rospy.Publisher("/topic_name", message_type, queue_size=size)
该" /topic_name "指示发布者将被发布到该话题。 message_type 是在“ / topic_name ”上发布的消息类型。
ROS发布可以是同步的也可以是异步的:
- 同步发布意味着发布者将尝试发布到主题,但如果该主题由其他发布者发布,则可能会被阻止。在这种情况下,第二个发布者被阻止,直到第一个发布者将所有消息序列化到缓冲区,并且缓冲区已将消息写入每个主题的订阅者。这是一个 rospy.Publisher 的默认行为,如果 queue_size 未使用或设置参数None。
- 异步发布意味着发布者可以将消息存储在队列中,直到可以发送消息。如果发布的消息数超过队列大小,则删除最旧的消息。可以使用 queue_size 参数设置队列大小。
如上所述创建发布者后,一个使用指定的数据类型 message 可以发布如下::
pub1.publish(message)
有关ROS发布者的更多信息,请参阅此处的文档。
3.为Simple mover添加节点
调用此节点 simple_mover 。顾名思义,这个节点只有一个任务,那就是命令联合运动 simple_arm 节点。
为此,它必须将关节角度命令消息发布到以下主题:
Topic Name | /simple_arm/joint_1_position_controller/command |
---|---|
Message Type | std_msgs/Float64 |
Description | Commands joint 1 to move counter-clockwise, units in radians |
Topic Name | /simple_arm/joint_2_position_controller/command |
- | - |
Message Type | std_msgs/Float64 |
Description | Commands joint 2 to move counter-clockwise, units in radians |
翻译如下:
主题名称 | / simple_arm / joint_1_position_controller / command |
---|---|
消息类型 | std_msgs / Float64 |
说明 | 命令关节1逆时针移动,单位为弧度 |
主题名称 | / simple_arm / joint_2_position_controller / command |
- | - |
消息类型 | std_msgs / Float64 |
说明 | 命令关节2逆时针移动,单位为弧度 |
(1)添加脚本目录
要在python中创建新节点,必须首先在 simple_arm 包中创建 scripts 目录,因为它尚不存在。
$ cd ~/catkin_ws/src/simple_arm/
$ mkdir scripts
(2)创建一个新脚本
创建脚本目录后,可以将可执行脚本添加到包中。但是,为了rosrun找到它们,必须更改其权限以允许执行。让我们添加一个简单的bash脚本,将“Hello World”打印到控制台。
$ cd scripts
$ echo '#!/bin/bash' >> hello
$ echo 'echo Hello World' >> hello
报错:
没有权限,而且在我使用sudo命令后还是无法解决问题。
解决办法:
使用 touch 命令新建 hello.sh 文件:
sudo touch hello.sh
更改文件的权限
chmod 777 hello,sh
如图所示:
在对文件设置适当的执行权限,重建工作空间以及获取新创建的环境之后,将能够运行该脚本。
$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ rosrun simple_arm hello.sh
如图现在已添加了一个名为 hello.sh 脚本
(3)创建空的simple_mover节点脚本
创建 simple_mover 节点脚本,就按照刚才介绍的例子来完成就可以了。
$ cd ~/catkin_ws/src/simple_arm/scripts
$ sudo touch simple_mover
$ sudo chmod 777 simple_mover
这样我们就有了空的 simple_mover 节点脚本,然后我们就可以来编写这个脚本了。
4.编写simple_mover节点脚本
使用命令
$ cd ~/catkin_ws/src/simple_arm/scripts/
$ nano simple_mover
打开文本编辑器,然后我们就可以在其中编写python代码了。
代码如下:
#!/usr/bin/env python
import math
# rospy是ROS的官方Python客户端库。它提供了通过Python与ROS连接所需的大部分基本功能。
import rospy
# 从std_msgs包中,我们导入Float64,这是ROS中的原始消息类型之一。
from std_msgs.msg import Float64
def mover():
pub_j1 = rospy.Publisher('/simple_arm/joint_1_position_controller/command',
Float64, queue_size=10)
pub_j2 = rospy.Publisher('/simple_arm/joint_2_position_controller/command',
Float64, queue_size=10)
rospy.init_node('arm_mover')
rate = rospy.Rate(10)
start_time = 0
while not start_time:
start_time = rospy.Time.now().to_sec()
while not rospy.is_shutdown():
elapsed = rospy.Time.now().to_sec() - start_time
pub_j1.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
pub_j2.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
rate.sleep()
if __name__ == '__main__':
try:
mover()
except rospy.ROSInterruptException:
pass
代码解释:
import rospy
rospy是ROS的官方Python客户端库。它提供了通过Python与ROS连接所需的大部分基本功能。它具有用于创建节点,与主题,服务,参数等接口的接口。
from std_msgs.msg import Float64
从std_msgs包中,我们导入Float64,这是ROS中的原始消息类型之一。该std_msgs包还包含所有其他基本类型。在本脚本中,我们将向每个关节的位置命令主题发布Float64消息。
def mover():
pub_j1 = rospy.Publisher('/simple_arm/joint_1_position_controller/command',
Float64, queue_size= 10)
pub_j2 = rospy.Publisher('/simple_arm/joint_2_position_controller/command',
Float64, queue_size=10)
在mover功能的顶部,声明了两个发布者,一个用于关节1命令,一个用于关节2命令。此处,该 queue_size 参数用于确定在删除 messages 之前可能存储在发布者队列中的最大 messages 数。
rospy.init_node('arm_mover')
初始化客户端节点并将其注册到主节点。这里“ arm_mover ”是节点的名称。 init_node() 必须在调用任何其他rospy包函数之前调用。该参数 anonymous=True 确保节点始终拥有的唯一名称
rate = rospy.Rate(10)
rate 此处创建的对象的值为10Hz。速率用于限制某些循环在ROS中旋转的频率。选择过高的速率可能会导致不必要的高CPU使用率,而选择过低的值可能会导致较高的整体系统延迟。为ROS系统中的所有节点选择合理的值是一门艺术。
start_time = 0
while not start_time:
start_time = rospy.Time.now().to_sec()
start_time 用于确定已经过了多长时间。当使用带模拟时间的ROS时(正如这个例子), rospy.Time.now() 最初将返回0,直到收到关于 /clock 主题的第一条消息。 这就是为什么要设置 start_time 并连续轮询,直到返回一个非零值。
while not rospy.is_shutdown():
elapsed = rospy.Time.now().to_sec() - start_time
pub_j1.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
pub_j2.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
rate.sleep()
这是主循环。由于调用 ***rate.sleep()***,循环以大约10赫兹的速度运行。通过循环体的每次旅行将导致发布两个联合命令消息。关节角度采用正弦波采样,周期为10秒,幅度为 [ − π / 2 , + π / 2 ] [- π / 2 ,+ π / 2 ] [−π/2,+π/2]。当节点收到要关闭的信号时(从主控制器或通过控制台窗口中的SIGINT信号),将退出循环。
if __name__ == '__main__':
try:
mover()
except rospy.ROSInterruptException:
pass
如果name变量设置为“ main ”,表示正在直接执行此脚本,mover()则将调用该函数。这里的 try / except 模块很重要,因为rospy广泛使用异常。这里捕到的特殊例外是 ROSInterruptException 。当节点已发出关闭信号时,会引发此异常。如果在节点关闭之前可能需要进行某种清理,那就可以在这里完成。
更多信息请参考这里。
这样我们已经完成了一个使机械手简单运行的脚本,接下来便使用 ctrl+x ,选择yes, 然后按Enter保存文件。
5.运行simple_mover节点脚本
确保的工作区已经构建,并且它setup.bash已经被提取,因此可以使 simple_arm 按如下方式启动:
$ cd ~/catkin_ws
$ roslaunch simple_arm robot_spawn.launch
一旦ROS Master,Gazebo和我们所有相关节点启动并运行,我们才可以启动 simple_mover 。
打开一个新终端并键入以下命令:
$ cd ~/catkin_ws
$ source devel/setup.bash
$ rosrun simple_arm simple_mover
6.ROS Services
我们已经编写了第一个ROS节点,也知道了如何发布主题,并且可以通过发布到**/simple_arm/joint_2_position_controller/command**主题来控制机械臂。接下来使另一个被调用的节点 arm_mover ,它实现了 safe_move 服务以允许通过服务调用来控制机械臂。
(1)定义服务
ROS服务允许节点之间存在请求/响应通信。在提供服务的节点内,请求消息由函数或方法处理。成功处理请求后,提供服务的节点会将消息发送回请求者节点。在Python中,可以使用以下定义格式创建ROS服务:
service = rospy.Service('service_name', serviceClassName, handler)
这里 service_name 是给服务的名称。其他节点将使用此名称来指定他们向其发送请求的服务。
它 serviceClassName 来自存在服务定义的文件名。每个服务都有一个 .srv 文件中提供的定义; 这是一个文本文件,为请求和响应提供正确的消息类型。
handler 是处理传入服务消息的函数或方法的名称。每次调用服务时都会调用此函数,并将来自服务调用的消息 handler 作为参数传递给它。 handler 应返回相应的服务响应消息。
(2)使用服务
可以直接从命令行调用服务,或者在另一个节点内使用ROS服务。
定义一个 ServiceProxy ,它提供了向服务发送消息的接口:
service_proxy = rospy.ServiceProxy('service_name', serviceClassName)
ServiceProxy 然后可以使用一种方式发送请求, 如下:
msg = serviceClassNameRequest()
# update msg attributes here to have correct data
response = service_proxy(msg)
在上面的代码中,通过调用 serviceClassNameRequest() 方法创建新的服务消息。此方法由rospy提供,其名称通过附加 Request() 用于的名称给出 serviceClassName 。
由于消息是新的,因此应更新消息属性以获得适当的数据。接下来, service_proxy 可以使用消息调用,并存储响应。
有关传递数据的其他方法service_proxy,请参阅此处的ROS文档。