如何编写ROS节点（二）

---例程Arm Mover

1.Arm Mover

现在已经编写了第一个ROS c++节点!这不是一项简单的任务。得学不少东西才能走到这一步。

我们还有更多的事情要做:

• 自定义消息生成 Custom message generation

• 服务 Services

• 参数 Parameters

• 启动文件 Launch Files

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

Arm Mover描述

在许多方面，arm_mover与simple_mover非常相似。像simple_mover一样，它负责命令手臂移动。但是，arm_mover节点并不是简单地命令手臂遵循预定的轨迹，而是提供了safe_move服务，允许系统中的其他节点发送movement_commands。除了允许通过服务接口移动外，arm_mover还允许使用参数配置最小和最大关节角度。

创建一个新的服务定义

与服务的交互由两条消息组成。一个节点将请求消息传递给服务，服务将响应消息返回给这个节点。请求和响应消息类型的定义包含在包的根目录下的srv目录中的.srv文件中。

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

$ cd /home/workspace/catkin_ws/src/simple_arm/
$ mkdir srv
$ cd srv
$ gedit GoToPosition.srv

现在使用gedit编辑GoToPosition.srv，包含以下内容:

float64 joint_1
float64 joint_2
---
string msg_feedback

服务定义总是包含两个部分，用“——”行分隔。第一部分是请求消息的定义。这里，一个请求包含两个float64字段，分别对应simple_arm的关节。第二部分包含服务响应。响应只包含一个字段msg_feedback。msg_feedback字段的类型是字符串，负责指示手臂已经移动到一个新的位置。

注意:定义自定义消息类型非常相似。唯一的区别是消息定义位于包根目录的msg目录中，具有.msg扩展名，并且不包含——section分隔符。可以在ROS wiki上找到关于创建消息 （http://wiki.ros.org/msg）和服务（http://wiki.ros.org/srv）的更多详细信息。

修改CMakeLists.txt

提醒一下，为了让catkin生成c++库，允许在代码中使用消息，必须修改simple_arm的CMakeLists.txt文件。在/home/workspace/catkin_ws/src/simple_arm/中可以找到这个文件。

首先，取消add_service_files()宏的注释，使它看起来像这样:

add_service_files(
   FILES
   GoToPosition.srv
)

这告诉catkin添加新创建的服务文件。

然后，确保generate_messages()宏是未注释的:

generate_messages(
   DEPENDENCIES
   std_msgs  # Or other packages containing msgs
)

它的宏实际上负责生成代码。

要迫使ROS用c++ 11编译c++代码，包括下面这行代码:

add_compile_options(-std=c++11)

修改package.xml

现在已经更新了CMakeLists.txt文件，还有一个文件需要修改:package.xml。

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

现在，我们将关注依赖项，您已经了解了构建（编译）时的依赖项和运行时的包依赖项。

当rosdep搜索这些依赖项时，将解析package.xml文件。因此，请确保在package.xml中存在message_generation构建（编译）时的依赖项和message_runtime运行的依赖项。

<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>

<run_depend>controller_manager</run_depend>
<run_depend>effort_controllers</run_depend>
<run_depend>gazebo_plugins</run_depend>
<run_depend>gazebo_ros</run_depend>
<run_depend>gazebo_ros_control</run_depend>
<run_depend>joint_state_controller</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>xacro</run_depend>

有关package.xml的更多信息，请查看ROS Wiki（http://wiki.ros.org/catkin/package.xml）。

使用ROS检查服务

现在已经创建了GoToPosition服务文件，让我们确保ROS可以使用rossrv show命令看到它:

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ rossrv show GoToPosition

发现如下：

[simple_arm/GoToPosition]:
float64 joint_1
float64 joint_2
---
string msg_feedback

这表明ROS可以看到您的服务。

至今已完成了这么多!首先创建了GoToPosition.srv文件。然后，在CMakeLists.txt中添加它的依赖项。此外，还检查了package.xml中的构建和运行依赖项。最后，检查了ROS是否可以看到服务文件。现在，让我们转向arm_mover的代码。

2.Arm Mover: The Code（代码）

创建空的arm_mover节点脚本

创建arm_mover节点所采取的步骤与创建simple_mover节点所采取的步骤完全相同，只是节点本身的实际名称不同。

打开一个新终端，输入以下命令:

$ cd /home/workspace/catkin_ws/src/simple_arm/src/
$ gedit arm_mover.cpp

使用gedit编辑器创建并打开c++ arm_mover源代码,将下面的代码复制并粘贴到源代码中，并保存文件。

arm_mover.cpp

#include"ros/ros.h"
#include"simple_arm/GoToPosition.h"
#include<std_msgs/Float64.h>

// Global joint publisher variables
ros::Publisher joint1_pub, joint2_pub;

// This function checks and clamps the joint angles to a safe zone
std::vector<float> clamp_at_boundaries(float requested_j1, float requested_j2)
{
    // Define clamped joint angles and assign them to the requested ones
    float clamped_j1 = requested_j1;
    float clamped_j2 = requested_j2;

    // Get min and max joint parameters, and assigning them to their respective variables
    float min_j1, max_j1, min_j2, max_j2;
    // Assign a new node handle since we have no access to the main one
    ros::NodeHandle n2;
    // Get node name
    std::string node_name = ros::this_node::getName();
    // Get joints min and max parameters
    n2.getParam(node_name + "/min_joint_1_angle", min_j1);
    n2.getParam(node_name + "/max_joint_1_angle", max_j1);
    n2.getParam(node_name + "/min_joint_2_angle", min_j2);
    n2.getParam(node_name + "/max_joint_2_angle", max_j2);

    // Check if joint 1 falls in the safe zone, otherwise clamp it
    if (requested_j1 < min_j1 || requested_j1 > max_j1) {
        clamped_j1 = std::min(std::max(requested_j1, min_j1), max_j1);
        ROS_WARN("j1 is out of bounds, valid range (%1.2f,%1.2f), clamping to: %1.2f", min_j1, max_j1, clamped_j1);
    }
    // Check if joint 2 falls in the safe zone, otherwise clamp it
    if (requested_j2 < min_j2 || requested_j2 > max_j2) {
        clamped_j2 = std::min(std::max(requested_j2, min_j2), max_j2);
        ROS_WARN("j2 is out of bounds, valid range (%1.2f,%1.2f), clamping to: %1.2f", min_j2, max_j2, clamped_j2);
    }

    // Store clamped joint angles in a clamped_data vector
    std::vector<float> clamped_data = { clamped_j1, clamped_j2 };

    return clamped_data;
}

// This callback function executes whenever a safe_move service is requested
boolhandle_safe_move_request(simple_arm::GoToPosition::Request& req,
    simple_arm::GoToPosition::Response& res)
{

    ROS_INFO("GoToPositionRequest received - j1:%1.2f, j2:%1.2f", (float)req.joint_1, (float)req.joint_2);

    // Check if requested joint angles are in the safe zone, otherwise clamp them
    std::vector<float> joints_angles = clamp_at_boundaries(req.joint_1, req.joint_2);

    // Publish clamped joint angles to the arm
    std_msgs::Float64 joint1_angle, joint2_angle;

    joint1_angle.data = joints_angles[0];
    joint2_angle.data = joints_angles[1];

    joint1_pub.publish(joint1_angle);
    joint2_pub.publish(joint2_angle);

    // Wait 3 seconds for arm to settle
    ros::Duration(3).sleep();

    // Return a response message
    res.msg_feedback = "Joint angles set - j1: " + std::to_string(joints_angles[0]) + " , j2: " + std::to_string(joints_angles[1]);
    ROS_INFO_STREAM(res.msg_feedback);

    return true;
}

int main(int argc, char** argv)
{
    // Initialize the arm_mover node and create a handle to it
    ros::init(argc, argv, "arm_mover");
    ros::NodeHandle n;

    // Define two publishers to publish std_msgs::Float64 messages on joints respective topics
    joint1_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_1_position_controller/command", 10);
    joint2_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_2_position_controller/command", 10);

    // Define a safe_move service with a handle_safe_move_request callback function
    ros::ServiceServer service = n.advertiseService("/arm_mover/safe_move", handle_safe_move_request);
    ROS_INFO("Ready to send joint commands");

    // Handle ROS communication events
    ros::spin();

    return0;
}

代码解释

#include"ros/ros.h"
#include"simple_arm/GoToPosition.h"
#include<std_msgs/Float64.h>

arm_mover包含的模块与simple_arm相同，只有一个新文件例外,即GoToPosition.h头文件，它是由我们之前创建的GoToPosition.srv文件生成的。

ros::init(argc, argv, "arm_mover");
ros::NodeHandle n;

在c++主函数中，初始化arm_mover节点并实例化一个ROS NodeHandle对象n以与ROS通信。

joint1_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_1_position_controller/command", 10);
joint2_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_2_position_controller/command", 10);

与前面在simple_arm节点中所做的一样，创建了两个发布者对象，将关节角度发布到手臂上。这些对象是全局定义的，以便从所有其他函数中轻松访问。

ros::ServiceServer service = n.advertiseService("/arm_mover/safe_move", handle_safe_move_request);

接下来，使用后跟safe_move的节点名创建GoToPosition服务。通常，先用该节点名命名服务，以便在大型项目中轻松找到它们。这个服务是用一个handle_safe_move_request回调函数定义的。该回调函数在接收到服务请求时运行。

ros::spin();

ros::spin()函数只是阻塞，直到节点接收到关闭请求。

boolhandle_safe_move_request(simple_arm::GoToPosition::Request& req, simple_arm::GoToPosition::Response& res)

当客户端向safe_move服务发送GoToPosition请求时，无论是从终端还是从单独的节点，handle_safe_move_request函数都会被调用。函数参数req的类型为GoToPosition::Request，服务响应参数res的类型为GoToPosition::Response。

std::vector<float> joints_angles = clamp_at_boundaries(req.joint_1, req.joint_2);

该函数将请求的角度传递给clamp_at_boundaries()函数。

std::vector<float> clamp_at_boundaries(float requested_j1, float requested_j2)
{
    // Define clamped joint angles and assign them to the requested ones
    float clamped_j1 = requested_j1;
    float clamped_j2 = requested_j2;

    // Get min and max joint parameters, and assign them to their respective variables
    float min_j1, max_j1, min_j2, max_j2;
    // Assign a new node handle since we have no access to the main one
    ros::NodeHandle n2;
    // Get node name
    std::string node_name = ros::this_node::getName();
    // Get joints min and max parameters
    n2.getParam(node_name + "/min_joint_1_angle", min_j1);
    n2.getParam(node_name + "/max_joint_1_angle", max_j1);
    n2.getParam(node_name + "/min_joint_2_angle", min_j2);
    n2.getParam(node_name + "/max_joint_2_angle", max_j2);

    // Check if joint 1 falls in the safe zone, otherwise clamp it
    if (requested_j1 < min_j1 || requested_j1 > max_j1) {
        clamped_j1 = std::min(std::max(requested_j1, min_j1), max_j1);
        ROS_WARN("j1 is out of bounds, valid range (%1.2f,%1.2f), clamping to: %1.2f", min_j1, max_j1, clamped_j1);
    }
    // Check if joint 2 falls in the safe zone, otherwise clamp it
    if (requested_j2 < min_j2 || requested_j2 > max_j2) {
        clamped_j2 = std::min(std::max(requested_j2, min_j2), max_j2);
        ROS_WARN("j2 is out of bounds, valid range (%1.2f,%1.2f), clamping to: %1.2f", min_j2, max_j2, clamped_j2);
    }

    // Store clamped joint angles in a clamped_data vector
    std::vector<float> clamped_data = { clamped_j1, clamped_j2 };

    return clamped_data;
}

clamp_at_boundaries()函数负责为每个关节强制设置最小和最大关节角度。如果传入的关节角度超出了可操作范围，它们将被“夹紧”到最近的允许值。每次调用clamp_at_boundaries时，都会从参数服务器检索最小和最大关节角度。这个函数的其余部分只是在必要时夹住关节角度。如果请求的关节角度超出边界，则会记录警告消息。

std_msgs::Float64 joint1_angle, joint2_angle;

joint1_angle.data = joints_angles[0];
joint2_angle.data = joints_angles[1];

joint1_pub.publish(joint1_angle);
joint2_pub.publish(joint2_angle);

然后，handle_safe_move_request()函数将夹紧的关节角度发布到手臂上。

ros::Duration(3).sleep();

safe_move服务将被阻塞3秒，以便手臂有足够的时间移动到请求的位置。

res.msg_feedback = "Joint angles set - j1: " + std::to_string(joints_angles[0]) + " , j2: " + std::to_string(joints_angles[1]);
ROS_INFO_STREAM(res.msg_feedback);

最后，safe_move服务返回一条消息，表明手臂已移动到新位置，并显示夹紧的关节角度。

下一步

现在已经编写了arm_mover节点，下一步是构建，启动，并通过命令行测试!

3.arm_mover:构建，启动和互动

修改CMakeLists.txt

在编译arm_move .cpp代码之前，必须包含编译器的指令。打开位于 /home/workspace/catkin_ws/src/simple_arm/ 的simple_arm包cmakelsts .txt文件，并在文件底部添加以下说明:

add_executable(arm_mover src/arm_mover.cpp)
target_link_libraries(arm_mover ${catkin_LIBRARIES})
add_dependencies(arm_mover simple_arm_generate_messages_cpp)

构建包

现在已经编写了arm_mover c++脚本，并包含了编译器的特定指令，让我们构建这个包:

$ cd /home/workspace/catkin_ws/
$ catkin_make

使用新服务启动项目

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

启动文件(如果存在)位于catkin包的根目录中的启动目录中。在启动文件中，可以指示ROS Master运行哪些节点。此外，还可以为每个节点指定特定的参数（parameters）和参数（arguments）。因此，在包含多个节点或具有多个参数（parameters）和参数（arguments）的节点的ROS包中需要一个启动文件。这个启动文件可以在一个命令中运行所有节点:roslaunch package_name launch_file.launch。Simple_arm的启动文件位于/home/workspace/catkin_ws/src/simple_arm/launch中。

要启动arm_mover节点，添加以下内容:

<!-- The arm mover node -->
<nodename="arm_mover"type="arm_mover"pkg="simple_arm"output="screen">
  <rosparam>
    min_joint_1_angle: 0
    max_joint_1_angle: 1.57
    min_joint_2_angle: 0
    max_joint_2_angle: 1.0
  </rosparam>
</node>

在启动文件中，节点标签指定名称、类型、包名和输出通道。ROS参数指定最小和最大关节角度。关于启动文件格式的更多信息可以在ROS wiki的XML页面(roslaunch/XML - ROS Wiki)上找到。

测试新服务

现在已经构建了代码并修改了启动文件，可以对其进行测试了。

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

注意:在重新启动之前，需要确保已经退出了之前的roslaunch会话。

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ roslaunch simple_arm robot_spawn.launch

然后，在一个新的终端中，验证节点和服务确实已经启动。

$ rosnode list
$ rosservice list

检查服务(/arm_mover/safe_move)和节点(/arm_mover)是否按预期显示。如果没有出现，请检查roscore控制台中的日志。现在可以使用rosservice与服务交互。

要查看相机图像流，可以使用命令rqt_image_view(在ROS wiki的RQT页面(rqt - ROS Wiki)上了解更多关于rqt和相关工具的信息):

$ rqt_image_view /rgb_camera/image_raw

调整视图

照相机显示的图像是灰色的。这是可以预料到的，因为它是笔直向上，指向Gazebo世界的灰色天空。

为了将摄像机对准工作台上的编号块，需要将关节1和关节2旋转大约pi/2弧度。让我们试一试:

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ rosservice call /arm_mover/safe_move "joint_1: 1.57
joint_2: 1.57"

注意:rosservice调用可以用制表符补全请求消息，这样就不必担心手写它了。另外，确保在两个关节参数之间包含换行符。

在输入命令时，应该看到手臂移动并最终停止，并向控制台报告新的关节夹角。这是意料之中的。

出乎意料的是手臂的位置。查看roscore控制台，我们可以看到问题所在：关节2要求的角度超出安全范围，因此被夹紧。我们要求1.57弧度，但最大关节角度设置为1.0弧度。

通过在参数服务器上设置max_joint_2_angle，我们应该能够增加关节2的最大角度，并在下次请求服务时将块显示在视图中。要更新该参数，请使用命令rosparam

$ rosparam set /arm_mover/max_joint_2_angle 1.57

现在我们应该能够移动手臂，这样所有的块都在摄像机的视野范围内:

$ rosservice call /arm_mover/safe_move "joint_1: 1.57 joint_2: 1.57"

结果出来了,所有的方块都在视野范围内!

arm_mover GitHub分支

可以从GitHub repo(GitHub - udacity/RoboND-simple_arm at arm_mover)下载该分支的副本。

4.ROS客户端和订阅者

在编写arm_mover节点时，练习了生成自定义消息、发布到主题、构建ROS服务服务器、设置参数和创建启动文件。现在几乎已经对ROS有了一个完整的概述，但是仍然需要了解ROS客户机（clients），以便从客户机节点请求服务，以及ROS订阅者（subscribers）。

ROS的客户（Clients）

在服务客户机节点中定义的服务客户机可以向服务服务器节点请求服务。在c++中，ROS客户端通常有以下格式，尽管其他参数（parameters）和参数（arguments）也是可能的:

ros::ServiceClient client = n.serviceClient<package_name::service_file_name>("service_name");

客户端对象从ros::ServiceClient类实例化。该对象允许通过调用client.call()函数来请求服务。

要在c++中与ROS Master通信，您需要一个NodeHandle（节点句柄），节点句柄n将初始化节点。

package_name::service_file_name表示位于包的srv目录下的服务文件的名称。

service_name参数表示在服务服务器节点中定义的服务名称。

ROS订阅者

订阅者使节点能够从主题中读取消息，从而允许将有用的数据流传输到节点。在c++中，ROS订阅者通常有以下格式，尽管其他参数（parameters）和参数（arguments）也是可能的：

ros::Subscriber sub1 = n.subscribe("/topic_name", queue_size, callback_function);

sub1对象是从ros:: subscriber类实例化的订阅者对象。该对象允许通过调用subscribe()函数订阅消息。

要在c++中与ROS Master通信，需要一个NodeHandle(节点句柄),节点句柄n将初始化节点。

“/topic_name”表示订阅服务器应该侦听的主题。

queue_size决定了可以存储在队列中的消息数量。如果发布的消息数量超过队列的大小，则删除最老的消息。例如，如果queue_size设置为100，并且队列中存储的消息数量等于100，将不得不开始删除旧消息，以便在队列中为新消息腾出空间。这意味着我们不能足够快地处理消息，可能需要增加queue_size。

callback_function是将运行每个传入消息的函数的名称。每次消息到达时，它都作为参数传递给callback_function。通常，此函数对传入数据执行有用的操作。注意，与服务处理函数不同，callback_function不需要返回任何东西。

有关订阅者的更多信息，请参阅ROS wiki上的订阅者文档(roscpp: ros::Subscriber Class Reference)。让我们转到look_away节点，这样就可以看到订阅者和客户端如何工作!