更复杂的ROS节点

1. Arm_mover节点

为了打好更好的基础，这是在Arm_mover节点还需要学习的内容

• 自定义消息生成
• 服务
• 参数
• 启动文件

为了理解上述内容，我们将编写另一个名为arm_mover的节点。

（1）Arm Mover的描述

在很多方面， arm_mover 都非常相似simple_mover。就像 simple_mover，它负责指挥机械臂移动。然而，代替简单地命令遵机械臂循预定轨迹，arm_mover 节点提供服务move_arm ，其允许系统中的其他节点发送 movement_commands 。

除了允许通过服务接口进行移动之外， arm_mover 还允许通过使用参数来配置最小和最大关节角度。

（2）创建新的服务定义

如前所述，与服务的交互包含两个传递的消息。传递给服务的请求以及从服务接收的响应。请求和响应消息类型的定义包含在包含srv根目录下的目录中的.srv文件中。

让我们为 simple_arm 定义一个新服务。我们称之为GoToPosiion。

$ cd ~/catkin_ws/src/simple_arm/
$ mkdir srv
$ cd srv
$ touch GoToPosition.srv

编辑 GoToPosition.srv ，使它包含以下内容：

float64 joint_1
float64 joint_2
---
duration time_elapsed

服务定义始终包含两个部分，以“---”行分隔。

第一部分是请求消息的定义。这里，请求包含两个 float64 字段，每个字段用于一个 simple_arm 关节。

第二部分包含服务响应。响应只包含一个字段time_elapsed。该time_elapsed字段具有持续时间类型，并且负责指示臂执行移动所花费的时间。

注意：定义自定义消息类型非常相似，唯一的区别是消息定义存在于msg包根目录中，具有“ .msg ”扩展名，而不是.srv，并且不包含“—”部分分频器。

可以在此处和此处找到有关创建消息和服务的更多详细信息。

（3）修改CMakeLists.txt

为了让 catkin 生成允许在代码中使用消息的python模块或C ++库，须首先修改simple_arm的 CMakeLists.txt。

$ cd ~/catkin_ws/src/simple_arm/
$ nano CMakeLists.txt

CMake是基于catkin的构建工具，CMakeLists.txt 它只不过是catkin使用的CMake脚本。

首先，确保find_package()宏列表std_msgs和message_generation所需的包。该find_package()宏应如下所示：

find_package(catkin REQUIRED COMPONENTSstd_msgsmessage_generation
)

正如名称所示的那样，std_msgs包中包含所有基本消息类型，并且message_generation需要为所有支持的语言（cpp，lisp，python，javascript）生成消息库。

注意：

在CMakeLists.txt中，可能还会看到controller_manager列为必需的包。实际上，这个包不是必需的,可以将其从“必需组件”列表中删除。

接下来，取消注释已注释掉的add_service_files():

## Generate services in the 'srv' folder
add_service_files(FILESGoToPosition.srv
)

这告诉catkin哪些文件生成代码。

最后，确保 generate_messages() 取消注释，如下所示：

generate_messages(DEPENDENCIESstd_msgs  # Or other packages containing msgs
)

这个宏实际上负责生成代码。

文件 CMakeLists.txt 是用于构建软件包的CMake构建系统的输入。任何符合CMake的包都包含一个或多个 CMakeLists.txt 文件，该文件描述了如何构建代码以及将代码安装到何处。用于catkin项目的 CMakeLists.txt 文件是标准的vanilla CMakeLists.txt文件，其中包含一些其他约束。更多关于 CmakeLists.txt 信息请查阅此处。

（4）修改package.xml

现在该 CMakeLists.txt 文件已被覆盖，现在应该在技术上能够构建该项目。但是，还有一个文件需要修改，即 package.xml 。

package.xml 负责定义许多软件包的属性，例如软件包的名称，版本号，作者，维护者和依赖项。

现在主要依赖考虑关系。之前已经说过了构建时依赖性和运行时包依赖性。在rosdep搜索这些依赖项时， package.xml 是正在解析的文件。现在添加message_generation和message_runtime依赖。

  catkinmessage_generationcontroller_managereffort_controllersgazebo_pluginsgazebo_rosgazebo_ros_controljoint_state_controllerjoint_state_publisherrobot_state_publishermessage_runtimexacro

其中只有两行作出添加：

  message_generationmessage_runtime

现在已准备好构建包。

有关更多信息package.xml，请查看ROS Wiki。

（5）构建包

如果成功构建工作区，那么GoToPosition在 devel 目录的深层创建了一个包含新服务模块的python包。

$ cd ~/catkin_ws
$ catkin_make
$ cd devel/lib/python2.7/dist-packages
$ ls

在获取新创建的资源后 setup.bash ，新的 simple_arm 软件包现已成为PYTHONPATH环境变量的一部分，并且可以使用。

$ env | grep PYTHONPATH

（6）创建空的arm_mover节点脚本

创建 arm_mover 节点所采取的步骤与创建 simple_mover 脚本所采取的步骤完全相同，但脚本本身的实际名称除外。

$ cd ~/catkin_ws
$ cd src/simple_arm/scripts
$ sudo touch arm_mover
$ sudo chmod 777 arm_mover

之前没有提chmod 777 arm_mover命令的解释，其解释在此处。

2.arm_mover相关代码

(1)完整代码

#!/usr/bin/env pythonimport math
import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from simple_arm.srv import *# 如果关节位置接近目标，则此函数返回True
def at_goal(pos_j1, goal_j1, pos_j2, goal_j2):tolerance = .05result = abs(pos_j1 - goal_j1) <= abs(tolerance)result = result and abs(pos_j2 - goal_j2) <= abs(tolerance)return result# 限制每个关节最小和最大关节角度
def clamp_at_boundaries(requested_j1, requested_j2):clamped_j1 = requested_j1clamped_j2 = requested_j2min_j1 = rospy.get_param('~min_joint_1_angle', 0)max_j1 = rospy.get_param('~max_joint_1_angle', 2*math.pi)min_j2 = rospy.get_param('~min_joint_2_angle', 0)max_j2 = rospy.get_param('~max_joint_2_angle', 2*math.pi)if not min_j1 <= requested_j1 <= max_j1:clamped_j1 = min(max(requested_j1, min_j1), max_j1)rospy.logwarn('j1 is out of bounds, valid range (%s,%s), clamping to: %s',min_j1, max_j1, clamped_j1)if not min_j2 <= requested_j2 <= max_j2:clamped_j2 = min(max(requested_j2, min_j2), max_j2)rospy.logwarn('j2 is out of bounds, valid range (%s,%s), clamping to: %s',min_j2, ，max_j2, clamped_j2)return clamped_j1, clamped_j2# 命令机械臂的移动，并返回移动时间 
def move_arm(pos_j1, pos_j2):time_elapsed = rospy.Time.now()j1_publisher.publish(pos_j1)j2_publisher.publish(pos_j2)while True:joint_state = rospy.wait_for_message('/simple_arm/joint_states', JointState)if at_goal(joint_state.position[0], pos_j1, joint_state.position[1], pos_j2):time_elapsed = joint_state.header.stamp - time_elapsedbreakreturn time_elapsed# 服务处理函数
def handle_safe_move_request(req):rospy.loginfo('GoToPositionRequest Received - j1:%s, j2:%s',req.joint_1, req.joint_2)clamp_j1, clamp_j2 = clamp_at_boundaries(req.joint_1, req.joint_2)time_elapsed = move_arm(clamp_j1, clamp_j2)return GoToPositionResponse(time_elapsed)def mover_service():rospy.init_node('arm_mover')service = rospy.Service('~safe_move', GoToPosition, handle_safe_move_request)rospy.spin()if __name__ == '__main__':j1_publisher = rospy.Publisher('/simple_arm/joint_1_position_controller/command',Float64, queue_size=10)j2_publisher = rospy.Publisher('/simple_arm/joint_2_position_controller/command',Float64, queue_size=10)try:mover_service()except rospy.ROSInterruptException:pass

（2）代码解释

#!/usr/bin/env pythonimport math
import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from simple_arm.srv import *

对于导入模块arm_mover和 simple_arm 是一样的，除了有两个新的输入。即，JointState 信息和simple_arm.srv模块。

JointState 消息发布到 /simple_arm/joint_states 主题，并用于监视机械臂的位置。

作为构建过程的一部分，simple_arm 包和srv模块由catkin自动生成。

———————————————————————————————————————————————————————

def at_goal(pos_j1, goal_j1, pos_j2, goal_j2):tolerance = .05result = abs(pos_j1 - goal_j1) <= abs(tolerance)result = result and abs(pos_j2 - goal_j2) <= abs(tolerance)return result

如果关节位置接近目标，则此函数返回True。在现实世界中，当从传感器进行测量时，总会有一些噪音。gazebo模拟器报告的关节位置也是如此。如果两个关节位置都在目标的0.05弧度内，则返回True。

———————————————————————————————————————————————————————

def clamp_at_boundaries(requested_j1, requested_j2):clamped_j1 = requested_j1clamped_j2 = requested_j2

clamp_at_boundaries()负责为每个关节强制设置最小和最大关节角。如果输入的角度超出了可允许范围，则将其限制到最近的允许值。

———————————————————————————————————————————————————————

    min_j1 = rospy.get_param('~min_joint_1_angle', 0)max_j1 = rospy.get_param('~max_joint_1_angle', 2*math.pi)min_j2 = rospy.get_param('~min_joint_2_angle', 0)max_j2 = rospy.get_param('~max_joint_2_angle', 2*math.pi)

每次调用clamp_at_boundaries()时，从参数服务器检索最小和最大关节角度。“~”是私有名称空间限定符，表示我们希望获得的参数位于此节点的私有名称空间/arm_mover/中。

(例如~min_joint_1_angle解析为/arm_mover/min_joint_1_angle)

第二个参数是要返回的默认值，在rospy.get_param()无法从param服务器获取参数的情况下。

———————————————————————————————————————————————————————

    if not min_j1 <= requested_j1 <= max_j1:clamped_j1 = min(max(requested_j1, min_j1), max_j1)rospy.logwarn('j1 is out of bounds, valid range (%s,%s), clamping to: %s',min_j1, max_j1, clamped_j1)if not min_j2 <= requested_j2 <= max_j2:clamped_j2 = min(max(requested_j2, min_j2), max_j2)rospy.logwarn('j2 is out of bounds, valid range (%s,%s), clamping to: %s',min_j2, max_j2, clamped_j2)return clamped_j1, clamped_j2

这个函数的其余部分只是在必要时约束关节角度。如果请求的关节角度超出界限，则记录警告消息。

———————————————————————————————————————————————————————

def move_arm(pos_j1, pos_j2):time_elapsed = rospy.Time.now()j1_publisher.publish(pos_j1)j2_publisher.publish(pos_j2)while True:joint_state = rospy.wait_for_message('/simple_arm/joint_states', JointState)if at_goal(joint_state.position[0], pos_j1, joint_state.position[1], pos_j2):time_elapsed = joint_state.header.stamp - time_elapsedbreakreturn time_elapsed

move_arm()命令arm移动，返回机械臂在移动时所需要的时间。

注意：

在这个函数中，我们使用了rospy.wait_for_message()调用来接收来自/simple_arm/joint_states主题的JointSate消息。这是阻塞函数调用，这意味着在收到/simple_arm/joint_states主题上的消息之前，它不会返回。

一般来说，不应该使用wait_for_message()。为了清楚起见，在这里简单地使用它，因为从handle_safe_move_request()函数调用 move_arm ，这要求将响应消息作为返回参数传回。

———————————————————————————————————————————————————————

def handle_safe_move_request(req):rospy.loginfo('GoToPositionRequest Received - j1:%s, j2:%s',req.joint_1, req.joint_2)clamp_j1, clamp_j2 = clamp_at_boundaries(req.joint_1, req.joint_2)time_elapsed = move_arm(clamp_j1, clamp_j2)return GoToPositionResponse(time_elapsed)

这是服务处理函数。当服务客户端向服务发送 GoToPosition 请求消息时，将safe_move调用此函数。函数参数req是类型 GoToPositionRequest 。服务响应属于类型 GoToPositionResponse 。

这是服务处理程序函数，只要收到新的服务请求就会调用它。从该函数返回对服务请求的响应。

注意：

move_arm()被阻塞，在arm完成移动之前不会返回。传入的消息不能被处理，当arm执行它的移动命令时，python脚本中不能执行任何其他有用的工作。虽然对于本例来说，这不会造成真正的问题，但是通常应该避免这种做法。避免阻塞执行线程的一个好方法是使用Action。下面是一些信息文档，描述了什么时候最好使用主题、服务和操作。

———————————————————————————————————————————————————————

def mover_service():rospy.init_node('arm_mover')service = rospy.Service('~safe_move', GoToPosition, handle_safe_move_request)rospy.spin()

在这里，节点用名称“arm_mover”初始化，GoToPosition 服务用名称“safe_move”创建。正如前面提到的，“~”限定符标识safe_move属于这个节点的私有名称空间。得到的服务名称将是/arm_mover/safe_move。

service()调用的第三个参数是在接收到服务请求时应该调用的函数。最后，spin()只是阻塞函数，直到节点接收到关闭请求。如果没有包含这一行，将导致返回mover_service()，脚本将完成执行。

阻塞函数：阻塞函数是当这个函数不执行完，函数所在线程就一直停止在这里不动。

———————————————————————————————————————————————————————

if __name__ == '__main__':j1_publisher = rospy.Publisher('/simple_arm/joint_1_position_controller/command', Float64, queue_size=10)j2_publisher = rospy.Publisher('/simple_arm/joint_2_position_controller/command', Float64, queue_size=10)try:mover_service()except rospy.ROSInterruptException:pass

这部分代码类似于simple_mover()。

———————————————————————————————————————————————————————

3.Arm Mover的启动和互动

使用新服务启动项目

要使 arm_mover 节点和随附的 safe_move 服务与所有其他节点一起启动，需要修改 robot_spawn.launch 。

当启动文件存在时，它们位于catkin包根目录中的启动目录中。simple_arm 的启动文件位于~/catkin_ws/src/simple_arm/launch中。

要使 arm_mover 节点启动，只需添加以下内容：

  min_joint_1_angle: 0max_joint_1_angle: 1.57min_joint_2_angle: 0max_joint_2_angle: 1.0

有关启动文件格式的更多信息，请参见此处。

测试新服务

为此，启动 simple_arm ，验证 arm_mover 节点正在运行，并且列出 safe_move 服务:

注意：

roslaunch在重新启动之前，需要确保已退出上一个会话。

$ cd ~/catkin_ws
$ catkin_mak