四、节点（node）

📌节点可理解为机器人身上的各个模块

1.Demo尝试

打开第一个终端

# 运行一个用c++构建的talker节点
ros2 run demo_nodes_cpp talker

打开另一个终端

# 运行一个用python构建的listener节点
ros2 run demo_nodes_py listener

python的listener节点即可收到来自cpp的talker节点的消息

这个例子同时也用到了“话题”功能，将在下节详细讲解

2.动手实现一个节点

本次用Python和C++两种语言来作演示，日后会以Python为主

(1)Python实现

创建软件包

cd ~/dev_ws/src

ros2 pkg create --build-type ament_python learning_node_py

当前的项目结构

上面的箭头所指文件夹是编写源代码的位置

下面的箭头所指文件是软件包的构建配置文件

cd learning_node_py/learning_node_py

# 创建代码文件，也可以直接在VScode里操作
touch hello_node.py

编写代码

import rclpy
from rclpy.node import Node
import time

def main(args=None):
    rclpy.init(args=args) # 初始化
    node = Node("node_hello_node") # 定义节点名称

    while rclpy.ok(): # 当节点没有问题时持续循环
        node.get_logger().info("Hello Node") # 输出到日志
        time.sleep(0.5) # 控制每个循环直接间隔0.5秒

    node.destroy_node() # 销毁节点
    rclpy.shutdown()

配置节点

在软件包的构建配置文件中添加编写的节点

格式：节点名=包名.代码文件名:函数名

构建软件包

cd ~/dev_ws

colcon build

source install/local_setup.bash

运行节点

ros2 run learning_node_py node_hello_node 

优化代码

我们可以把代码写成面向对象的形式，提升可读性方便调试

新建一个文件hello_class.py

import rclpy
from rclpy.node import Node
import time

class HelloClassNode(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        while rclpy.ok():
            self.get_logger().info("Hello Node")
            time.sleep(0.5)

def main(args=None):
    rclpy.init(args=args)
    node = HelloClassNode("node_hello_class")
    node.destroy_node()
    rclpy.shutdown()

同样地，将节点添加进配置文件，节点之间以逗号分隔

构建并运行，效果和刚才一样

(2)C++实现

创建软件包

cd ~/dev_ws/src

ros2 pkg create --build-type ament_cmake learning_node_cpp

当前的项目结构

上面的箭头所指文件夹是编写源代码的位置

下面的箭头所指文件是软件包的构建配置文件

cd learning_node_cpp/src

# 创建代码文件，也可以直接在VScode里操作
touch hello_node.cpp

编写代码

#include "rclcpp/rclcpp.hpp"

class HelloNode : public rclcpp::Node
{
    public:
        HelloNode()
        : Node("node_hello_node")
        {
            while (rclcpp::ok())
            {
                RCLCPP_INFO(this->get_logger(), "Hello World");
                sleep(1);
            }
        }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<HelloNode>());
    rclcpp::shutdown();

    return 0;

配置节点

构建软件包

cd ~/dev_ws

colcon build

source install/local_setup.bash

运行节点

ros2 run learning_node_cpp node_hello_node

3.进阶--实现人脸识别节点

（1）环境配置

sudo pip3 install opencv-python

（2）编写代码

import cv2
import rclpy
from rclpy.node import Node

class FaceDetectNode(Node):

    def __init__(self, node_name):
        super().__init__(node_name)
        self.face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml')
        self.cap = cv2.VideoCapture(0)

        while rclpy.ok():
            ret, frame = self.cap.read()
            # 镜像翻转
            frame = cv2.flip(frame, 1)
            if ret:
                quit = self.detect(frame)

                # 判断是否按下esc键
                if quit == 27:
                    self.cap.release()
                    cv2.destroyAllWindows()
                    break

    def detect(self, frame):
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        faces = self.face_cascade.detectMultiScale(gray, scaleFactor=1.1, minNeighbors=5, minSize=(30, 30))

        # 框出人脸
        for (x, y, w, h) in faces:
            cv2.rectangle(frame, (x, y), (x+w, y+h), (255, 0, 0), 2)

        cv2.imshow('Face Detection', frame)
        return cv2.waitKey(1) & 0xFF

def main(args=None):
    rclpy.init(args=args)
    node = FaceDetectNode("face_detect_node")
    node.destroy_node()
    rclpy.shutdown()