编写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 pythonimport math
# rospy是ROS的官方Python客户端库。它提供了通过Python与ROS连接所需的大部分基本功能。
import rospy
# 从std_msgs包中，我们导入Float64，这是ROS中的原始消息类型之一。
from std_msgs.msg import Float64def 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 = 0while not start_time:start_time = rospy.Time.now().to_sec()while not rospy.is_shutdown():elapsed = rospy.Time.now().to_sec() - start_timepub_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 = 0while 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_timepub_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秒，幅度为$[-\pi /2，+\pi /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文档。