如何编写ROS节点（一）

---例程simple_mover

1.概述

在本文中，将学习如何在c++中编写ROS节点。

编写的第一个节点称为simple_mover。simple_mover节点只向simple_arm发布关节角度命令。

理解ROS节点的一般结构后，将编写另一个名为arm_mover的节点。arm_mover节点提供了一个名为safe_move的服务，该服务允许将手臂移动到工作区中被认为安全的任何位置。安全区域由最小和最大关节角度限定，并可通过ROS参数服务器进行配置。

本文编写的第3个节点是look_away节点。该节点订阅了手臂关节位置和一个主题，相机数据将在其中发布。当摄像头检测到一个颜色一致的图像时，意味着它正在看顶空，而手臂没有移动，节点将通过客户端调用safe_move服务将手臂移动到一个新的位置。

2.ROS发布者

在看到simple_mover的代码之前，了解ROS publisher在c++中的工作方式可能会有所帮助。发布者允许节点向主题发送消息，这样来自节点的数据就可以用于ROS的其他部分。在c++中，ROS发布者通常有以下定义格式，尽管其他参数和参数也是可能的:

ros::Publisher pub1 = n.advertise<message_type>("/topic_name", queue_size);

pub1对象是一个从ros:: publisher类实例化的发布者对象。该对象允许通过调用publish()函数来发布消息。要在c++中与ROS master通信，需要一个NodeHandle。节点句柄n将完全初始化该节点。advertise()函数用于与ROS通信，并通知希望在给定的主题名称上发布消息。“/topic_name”表示发布者将发布到哪个主题。message_type是发布在“/topic_name”上的消息的类型。例如，ROS中的字符串消息数据类型为std_msgs:: string。queue_size表示可以存储在队列中的消息数量。发布者可以将消息存储在队列中，直到消息可以发送为止。如果存储的消息数量超过队列的大小，则删除最老的消息。一旦创建了发布者对象pub1，就可以按照如下方式发布具有指定数据类型的消息:

pub1.publish(msg);

有关c++ ROS发布者的更多信息，请参阅这里的文档http://docs.ros.org/en/jade/api/roscpp/html/classros_1_1Publisher.html

3.第一个节点：Simple Mover

现在，将使用c++实现第一个ROS节点。这个节点称为simple_mover。顾名思义，这个节点只有一个职责，那就是为simple_arm命令关节运动。

目标

simple_mover节点的目标是命令简单臂中的每个关节，并使其随时间在-pi/2到pi/2之间摆动。下面是该节点运行的演示:

话题

为此，它必须向以下话题发布关节角度命令消息:

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

注意:需要创建一个catkin_ws包，并在/home/workspace/catkin_ws/src中克隆这个包:

$ mkdir -p /home/workspace/catkin_ws/src/
$ cd /home/workspace/catkin_ws/src/
$ git clone -b first_interaction https://github.com/udacity/RoboND-simple_arm/ simple_arm

添加源目录

为了在c++中创建一个新节点，必须首先在simple_arm包中创建src目录，因为它还不存在。

$ cd /home/workspace/catkin_ws/src/simple_arm/
$ mkdir src

创建一个新脚本

一旦创建了源目录，就可以将c++脚本添加到包中。现在，在包的源目录中创建simple_mover c++脚本。

$ cd /home/workspace/catkin_ws/src/simple_arm/src/
$ touch simple_mover.cpp

4.Simple Mover: The Code

下面是simple_mover c++节点的完整代码，并嵌入了逐行注释。可以将这段代码复制并粘贴到/home/workspace/catkin_ws/src/simple_arm/src/目录中创建的simple_mover脚本，如下所示，

首先，打开一个新终端。然后:

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

使用gedit编辑器打开了c++ simple_mover脚本，现在将下面的代码复制并粘贴到脚本中，并保存脚本。我鼓励您编写这段代码，而不是复制它，以便您更熟悉语法。

simple_mover.cpp

#include"ros/ros.h"
#include"std_msgs/Float64.h"

intmain(int argc, char** argv)
{
    // Initialize the arm_mover node
    ros::init(argc, argv, "arm_mover");

    // Create a handle to the arm_mover node
    ros::NodeHandle n;

    // Create a publisher that can publish a std_msgs::Float64 message on the /simple_arm/joint_1_position_controller/command topic
    ros::Publisher joint1_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_1_position_controller/command", 10);
    // Create a publisher that can publish a std_msgs::Float64 message on the /simple_arm/joint_2_position_controller/command topic
    ros::Publisher joint2_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_2_position_controller/command", 10);

    // Set loop frequency of 10Hz
    ros::Rate loop_rate(10);

    int start_time, elapsed;

    // Get ROS start time
    while (not start_time) {
        start_time = ros::Time::now().toSec();
    }

    while (ros::ok()) {
        // Get ROS elapsed time
        elapsed = ros::Time::now().toSec() - start_time;

        // Set the arm joint angles
        std_msgs::Float64 joint1_angle, joint2_angle;
        joint1_angle.data = sin(2 * M_PI * 0.1 * elapsed) * (M_PI / 2);
        joint2_angle.data = sin(2 * M_PI * 0.1 * elapsed) * (M_PI / 2);

        // Publish the arm joint angles
        joint1_pub.publish(joint1_angle);
        joint2_pub.publish(joint2_angle);

        // Sleep for the time remaining until 10 Hz is reached
        loop_rate.sleep();
    }

    return0;
}

代码解释

#include"ros/ros.h"

ros是ROS的官方客户端库。它提供了通过c++与ROS接口所需的大部分基本功能。它具有用于创建节点和与主题、服务和参数交互的工具。

#include"std_msgs/Float64.h"

从std_msgs包中导入Float64头文件。std_msgs (http://wiki.ros.org/std_msgs)包还包含ROS中的基本消息类型。稍后，将发布Float64消息到每个关节的位置命令主题。

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

ROS节点使用init()函数初始化，并向ROS Master注册。这里arm_mover是节点的名称。注意，main函数同时接受argc和argv参数，并将它们传递给init()函数。

ros::NodeHandle n;

从NodeHandle类实例化一个节点句柄对象n。这个节点句柄对象将完全初始化节点，并允许它与ROS Master通信。

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

声明了两个发布者，一个用于关节1命令，另一个用于关节2命令。节点句柄将告诉ROS主机将在关节主题上发布一条Float64消息。节点句柄还在advertise函数的第二个参数中将队列大小设置为10。

ros::Rate loop_rate(10);

使用loop_rate对象设置10HZ的频率。在ROS中，速率用于限制某些循环的频率。选择过高的速率可能导致不必要的CPU占用，而选择过低的速率可能导致较高的延迟。为ROS系统中的所有节点选择合理的值是一门艺术。

start_time = ros::Time::now().toSec();

我们将start_time设置为当前时间。稍后我们将使用它来确定已经过了多长时间。当使用模拟时间的ROS时(就像我们在这里所做的那样)，ROS - time -now最初将返回0，直到收到关于/clock主题的第一条消息。这就是为什么设置start_time并连续轮询直到返回非零值的原因。

elapsed = ros::Time::now().toSec() - start_time;

在主循环中，通过测量当前时间并减去开始时间来计算经过的时间。

std_msgs::Float64 joint1_angle, joint2_angle;
joint1_angle.data = sin(2 * M_PI * 0.1 * elapsed) * (M_PI / 2);
joint2_angle.data = sin(2 * M_PI * 0.1 * elapsed) * (M_PI / 2);

关节角从周期为10秒的正弦波中采样，大小为[-pi/2， +pi/2]。

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

每次遍历循环体都将导致发布两个关节的命令消息。

loop_rate.sleep();

由于调用loop_rate.sleep()，循环以大约10赫兹的速度遍历。当节点接收到关闭信号(来自ROS Master或通过控制台窗口的信号)时，循环将退出。

5.Simple Mover: Build and Run

在运行simple_mover节点之前，必须编译c++脚本。

修改CMakeLists.txt

为了让catkin生成c++库，首先修改simple_arm的CMakeLists.txt。

CMake是基于catkin的构建工具，CMakeLists.txt是catkin使用的CMake脚本。如果您熟悉makefile的概念，这是类似的。

导航到包CMakeLists.txt文件并打开它:

$ cd /home/workspace/catkin_ws/src/simple_arm/
$ gedit CMakeLists.txt 

首先，确保find_package()宏列出了std_msgs、message_generation和controller_manager作为必需的包。find_package()宏应该如下所示:

find_package(catkin REQUIRED COMPONENTS
        std_msgs
        message_generation
        controller_manager
)

顾名思义，std_msgs包包含所有基本消息类型，并且需要message_generation来为所有受支持的语言(cpp、lisp、python、javascript)生成消息库。controller_manager是另一个负责控制臂的包。

现在，在文件底部添加以下代码块:

include_directories(include ${catkin_INCLUDE_DIRS})

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

这些指令要求编译器包含你的c++代码的目录、可执行文件、链接库和依赖项:

add_executable(node_name sourcecode_directory)

创建可执行simple_mover文件。

target_link_libraries(node_name ${catkin_LIBRARIES})

这将把所有链接的库添加到编译器中。

add_dependencies(node_name package_name_generate_messages_cpp)

生成此包的消息头，然后才能使用它们。

请记住，无论何时要编写c++ ROS节点，都应该包含这些指令。有关CMakeLists.txt的更多信息，请查看ROS wiki上的CMakeLists.txt页面（http://wiki.ros.org/catkin/CMakeLists.txt）。

Building the Package构建包

现在已经包含了编译器的特定指令，让我们构建这个包:

$ cd /home/workspace/catkin_ws/
$ catkin_make

运行simple_mover

假设您的工作空间最近已经构建好，您可以像下面这样启动simple_arm:

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

ROS Master、Gazebo和所有相关节点启动并运行之后，我们就可以最终启动simple_mover了。为此，打开一个新终端并输入以下命令:

$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ rosrun simple_arm simple_mover

simple_mover GitHub branch

https://github.com/udacity/RoboND-simple_arm/tree/simple_mover

6.ROS服务（ROS Services）

现在已经编写了第一个ROS节点，看到了发布到主题的工作方式，并且能够通过发布到/simple_arm/joint_1_position_controller/command主题和/simple_arm/joint_2_position_controller/command主题来控制机械臂。接下来，我们将看到另一个名为arm_mover的节点，它实现了safe_move服务，以允许服务调用来控制手臂。

定义服务

ROS服务允许节点之间存在请求/响应通信。在提供服务的节点中，请求消息由函数或方法处理。成功处理请求后，提供服务的节点将消息发送回请求者节点。在c++中，ROS服务服务器可以使用以下定义格式创建:

ros::ServiceServer service = n.advertiseService(`service_name`, handler);

在ROS中，服务类名ServiceServer来自于服务定义所在的文件名。每个服务在.srv文件中提供一个定义;这是一个文本文件，为请求和响应提供适当的消息类型。

advertiseService()允许通过节点句柄n与ROS通信，并通知ROS所想要创建服务。

service_name是服务的名称。其他节点将使用此名称指定它们向其发送请求的服务。

handler（处理程序）是处理传入服务消息的函数或方法的名称。每次调用服务时都会调用此函数，来自服务调用的消息将作为参数传递给handler（处理程序）函数。handler（处理程序）应该返回适当的服务响应消息。

使用服务

命令行

服务可以直接从命令行调用，使用:

$ rosservice call service_name “request”

拨打服务电话后，您将等待应答。

ROS服务客户端

另一种方法是从节点内以编程方式使用ROS服务。定义一个ROS client（ROS客户端），提供了向服务发送消息的接口:

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

ROS client（ROS客户端）可以使用的一种方式是发送请求，如下所示:

client.call(srv);    // request a service 

现在，我们将重点关注如何创建ROS service server（服务服务器）。稍后，在look_away节点中，将练习从service client（服务客户机）节点调用服务。

有关如何创建和调用ROS服务的详细说明，请参阅有关服务的ROS文档

http://wiki.ros.org/roscpp/Overview/Services。

让我们从arm_mover代码开始，这样就可以看到如何将safe_move服务与节点中的发布者结合起来，以便每当请求一个服务时，它将负责发布关于主题的消息。