如何编写ROS节点（三）

---例程Look Away

1. Look Away的描述

要查看ROS订阅者和客户机的运行情况，需要编写一个名为look_away的节点。look_away节点订阅/rgb_camera/image_raw主题，其中包含来自安装在机械臂末端的摄像机的图像数据。每当相机指向一个不感兴趣的图像(在本例中，是一个颜色统一的图像)时，回调函数将请求safe_move服务来安全地将机械臂移动到感兴趣的图像上。代码中还有一些额外的部分以确保这个过程顺利执行，我们稍后将重点讨论这部分。

更新启动文件

就像对arm_mover节点所做的一样，要让look_away与其他节点一起启动，需要修改robot_spawn.launch，可以在/home/workspace/catkin_ws/src/simple_arm/ Launch中找到。在那里添加以下代码:

<!-- The look away node -->
<nodename="look_away"type="look_away"pkg="simple_arm"/>

记住关节转半圈需要pi/2弧度的旋转。数值上，pi/2约为1.57。因为我们希望通过一个请求就能让关节旋转一半，所以在arm_mover中设置max_joint_2_angle: 1.57会很有帮助:

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

2.Look Away代码

创建空的look_away节点脚本

创建look_away节点所采取的步骤与创建simple_mover和arm_mover脚本所采取的步骤完全相同，但是当然要更改文件本身的实际名称。

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

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

使用gedit编辑器创建并打开了c++ look_away文件，现在复制并粘贴下面的代码并保存文件。

look_away.cpp

#include "ros/ros.h"
#include "simple_arm/GoToPosition.h"
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Image.h>

// Define global vector of joints last position, moving state of the arm, and the client that can request services
std::vector<double> joints_last_position{ 0, 0 };
bool moving_state = false;
ros::ServiceClient client;

// This function calls the safe_move service to safely move the arm to the center position
void move_arm_center()
{
    ROS_INFO_STREAM("Moving the arm to the center");

    // Request centered joint angles [1.57, 1.57]
    simple_arm::GoToPosition srv;
    srv.request.joint_1 = 1.57;
    srv.request.joint_2 = 1.57;

    // Call the safe_move service and pass the requested joint angles
    if (!client.call(srv))
        ROS_ERROR("Failed to call service safe_move");
}

// This callback function continuously executes and reads the arm joint angles position
void joint_states_callback(const sensor_msgs::JointState js)
{
    // Get joints current position
    std::vector<double> joints_current_position = js.position;

    // Define a tolerance threshold to compare double values
    double tolerance = 0.0005;

    // Check if the arm is moving by comparing its current joints position to its latest
    if (fabs(joints_current_position[0] - joints_last_position[0]) < tolerance && fabs(joints_current_position[1] - joints_last_position[1]) < tolerance)
        moving_state = false;
    else {
        moving_state = true;
        joints_last_position = joints_current_position;
    }
}

// This callback function continuously executes and reads the image data
void look_away_callback(const sensor_msgs::Image img)
{

    bool uniform_image = true;

    // Loop through each pixel in the image and check if its equal to the first one
    for (int i = 0; i < img.height * img.step; i++) {
        if (img.data[i] - img.data[0] != 0) {
            uniform_image = false;
            break;
        }
    }

    // If the image is uniform and the arm is not moving, move the arm to the center
    if (uniform_image == true && moving_state == false)
        move_arm_center();
}

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

    // Define a client service capable of requesting services from safe_move
    client = n.serviceClient<simple_arm::GoToPosition>("/arm_mover/safe_move");

    // Subscribe to /simple_arm/joint_states topic to read the arm joints position inside the joint_states_callback function
    ros::Subscriber sub1 = n.subscribe("/simple_arm/joint_states", 10, joint_states_callback);

    // Subscribe to rgb_camera/image_raw topic to read the image data inside the look_away_callback function
    ros::Subscriber sub2 = n.subscribe("rgb_camera/image_raw", 10, look_away_callback);

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

    return 0;
}

代码解释

#include"ros/ros.h"
#include"simple_arm/GoToPosition.h"
#include<sensor_msgs/JointState.h>
#include<sensor_msgs/Image.h>

头文件与arm_mover中的头文件类似，只是这次我们包含了JointState.h头文件，以便我们可以读取机械臂关节的位置。我们还包括Image.h头文件，以便我们可以使用相机数据。

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

在c++主函数内部，look_away节点被初始化，ROS NodeHandle对象n被实例化以与ROS通信。

client = n.serviceClient<simple_arm::GoToPosition>("/arm_mover/safe_move");

这里创建了一个客户端对象。该节点可以从之前创建的arm_mover节点中的/arm_mover/safe_move服务请求GoToPosition服务。这个客户端对象在代码中是全局定义的，因此可以在任何函数中请求服务。特别是，这发生在move_arm_center()函数中。

ros::Subscriber sub1 = n.subscribe("/simple_arm/joint_states", 10, joint_states_callback);

第一个订阅者对象sub1订阅了/simple_arm/joint_states主题。通过订阅本主题，我们可以通过读取每个关节的角度来跟踪机械臂的位置。queue_size被设置为10，这意味着队列中最多可以存储10条消息。来自每个新传入消息的数据被传递给joint_states_callback函数。

ros::Subscriber sub2 = n.subscribe("rgb_camera/image_raw", 10, look_away_callback);

第二个订阅对象sub2订阅了/rgb_camera/image_raw主题。queue_size也被设置为10。每次有新消息到达时，都会调用look_away_callback函数。

ros::spin();

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

void joint_states_callback(const sensor_msgs::JointState js)
{
    // Get joints current position
    std::vector<double> joints_current_position = js.position;

    // Define a tolerance threshold to compare double values
    double tolerance = 0.0005;

    // Check if the arm is moving by comparing its current joints position to its latest
    if (fabs(joints_current_position[0] - joints_last_position[0]) < tolerance && fabs(joints_current_position[1] - joints_last_position[1]) < tolerance)
        moving_state = false;
    else {
        moving_state = true;
        joints_last_position = joints_current_position;
    }
}

当sub1接收到关于/simple_arm/joint_states主题的消息时，该消息被传递给变量js中的joint_states_callback。joint_states_callback()函数检查js中提供的当前关节状态是否与之前存储在全局joints_last_position变量中的关节状态相同。如果当前和之前的关节状态是相同的(取决于指定的错误公差)，那么机械臂已经停止移动，并且moving_state标志被设置为False。该标志是全局定义的，以便与代码中的其他函数共享。另一方面，如果当前和之前的关节状态不同，那么机械臂仍在运动。在本例中，函数将moving_state设置为true，并使用存储在joints_current_position中的当前位置数据更新joints_Last_position变量。

void look_away_callback(const sensor_msgs::Image img){

   bool uniform_image = true;

   // Loop through each pixel in the image and check if its equal to the first one
   for (int i = 0; i < img.height * img.step; i++) {
      if (img.data[i] - img.data[0] != 0) {
          uniform_image = false;
          break;
      }
   }

   // If the image is uniform and the arm is not moving, move the arm to the center
   if (uniform_image == true && moving_state == false)
      move_arm_center();
}

look_away_callback()函数的作用是:接收来自/rgb_camera/image_raw主题的图像数据（http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html）。回调函数（callback）首先检查图像中的所有颜色值是否与第一个像素的颜色值相同。如果图像是一致的，且机械臂没有移动，则调用move_arm_center()函数。

void move_arm_center(){
    ROS_INFO_STREAM("Moving the arm to the center");

    // Request centered joint angles [1.57, 1.57]
    simple_arm::GoToPosition srv;
    srv.request.joint_1 = 1.57;
    srv.request.joint_2 = 1.57;

    // Call the safe_move service and pass the requested joint angles
    if (!client.call(srv))
        ROS_ERROR("Failed to call service safe_move");
}

在move_arm_center函数中，使用arm_mover/safe_move服务，创建并发送一个GoToPosition请求消息，将两个关节角度移动到1.57弧度。

3.Look Away: 构建，启动和交互（Build, Launch and Interact）

修改CMakeLists.txt

在编译look_away.cpp代码之前，必须包含编译器的指令。作为提醒，对于编写的每个c++ ROS节点，必须在CMakeLists.txt文件中添加它的依赖项。打开simple_arm包的cmakelsts .txt文件，该文件位于/home/workspace/catkin_ws/src/simple_arm/，并在文件底部添加以下说明:

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

构建包

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

$ cd /home/workspace/catkin_ws/
$ catkin_make

启动节点

现在可以像之前一样启动simple_arm并与之交互:

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

与机械臂互动

在启动后，机械臂应该离开灰色的天空，看向积木。要查看相机图像流，可以使用与之前相同的命令:

$ rqt_image_view /rgb_camera/image_raw

为了检查一切是否正常工作，打开一个新终端并发送一个服务调用，将机械臂直接指向天空(注意消息中的换行是必要的):

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

look_away GitHub分支

通过访问GitHub repo（https://github.com/udacity/RoboND-simple_arm/）可下载包含所有三个节点的该实验的副本。

4.发布-订阅类

在本文的发布者和订阅者节点中，全局变量和对象被定义为可在代码中的任何地方使用。这样做是为了简化代码，但不是一个好的实践。事实上，应该始终编写一个发布-订阅类，以便轻松地与代码中的任何回调函数共享变量和对象。下面是一个可以使用的ROS发布-订阅模板类:

ROS Class C++ 代码

#include<ros/ros.h>

class SubscribeAndPublish
{
public:
  SubscribeAndPublish()
  {
    //Topic you want to publish
    pub_ = n_.advertise<PUBLISHED_MESSAGE_TYPE>("/published_topic", 1);

    //Topic you want to subscribe
    sub_ = n_.subscribe("/subscribed_topic", 1, &SubscribeAndPublish::callback, this);
  }

  voidcallback(const SUBSCRIBED_MESSAGE_TYPE& input){
    PUBLISHED_MESSAGE_TYPE output;
    //.... do something with the input and generate the output...
    pub_.publish(output);
  }

private:
  ros::NodeHandle n_; 
  ros::Publisher pub_;
  ros::Subscriber sub_;

};//End of class SubscribeAndPublish

int main(int argc, char **argv)
{
  //Initiate ROS
  ros::init(argc, argv, "subscribe_and_publish");

  //Create an object of class SubscribeAndPublish that will take care of everything
  SubscribeAndPublish SAPObject;

  ros::spin();

  return0;
}

在后续的文中，我们会使用这个模板类来实现相关的节点。