ROS方向第二次汇报（1）

1.本方向内学习内容：

1.1.节点：

节点的编码流程包括：

编程接口初始化
创建节点并初始化
实现节点功能
销毁节点并关闭接口

例如一个示例（发布‘’hello world日志信息‘’）(面向过程版)

import rclpy                              # ROS2 PY接口库
from rclpy.node import Node               # ROS2 节点类
import time 

def main(args=None):                      # ROS2节点主入口main函数
	rclpy.init(args=args)                 # ROS2 PY接口初始化
	node=Node("node_helloworld")          # 创建ROS2节点对象并初始化

	while reclpy.ok():                    # ROS2系统是否正常运行
	node.get_logger().info(“Hello World”) #ROS2日志输出
	time.sleep(0.5)

	node.destroy_node()                   # 销毁节点对象
	rclpy.shutdown()                      # 关闭ROS2 PY接口

如下是面向对象版本，封装了一个class：

import rclpy                              # ROS2 Python接口库
from rclpy.node import Node               # ROS2 节点类
import time

class HelloWorldNode(Node):
    def __init__(self, name):
        super().__init__(name)                       # ROS2节点父类初始化
        while rclpy.ok():                            # ROS2系统是否正常运行
            self.get_logger().info("Hello World")    # ROS2日志输出
            time.sleep(0.5)                          # 休眠控制循环时间

def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = HelloWorldNode("node_helloworld_class")   # 创建ROS2节点对象并进行初始化
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

注意事项：

注意：
1.在修改代码后需要重新编译才能运行修改后的程序！
2.注意创建工作空间，编译工作空间后一定要记得在install找到配置环境变量的文件并在bashrc.中完成环境变量配置source ~/wsname…，否则运行节点的时候会找不到功能包！！！（本人已踩坑，切勿遗漏该步骤）
3.在功能包中的setup.py中设置命令行主函数入口（main）
4.功能包是用命令创建，而不是如同创建工作空间一样直接建文件夹！！！

如下是另一个示例(通过摄像头检测红色物体)：

import rclpy                            # ROS2 Python接口库
from rclpy.node import Node             # ROS2 节点类

import cv2                              # OpenCV图像处理库
import numpy as np                      # Python数值计算库

lower_red = np.array([0, 90, 128])     # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255])  # 红色的HSV阈值上限

def object_detect(image):
    hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)                       # 图像从BGR颜色模型转换为HSV模型
    mask_red = cv2.inRange(hsv_img, lower_red, upper_red)                  # 图像二值化

    contours, hierarchy = cv2.findContours(mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测

    for cnt in contours:                                                   # 去除一些轮廓面积太小的噪声
        if cnt.shape[0] < 150:
            continue
            
        (x, y, w, h) = cv2.boundingRect(cnt)                               # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
        cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)                 # 将苹果的轮廓勾勒出来
        cv2.circle(image, (int(x+w/2), int(y+h/2)), 5, (0, 255, 0), -1)    # 将苹果的图像中心点画出来
	    
    cv2.imshow("object", image)                                            # 使用OpenCV显示处理后的图像效果
    cv2.waitKey(50)

def main(args=None):                                                       # ROS2节点主入口main函数
    rclpy.init(args=args)                                                  # ROS2 Python接口初始化
    node = Node("node_object_webcam")                                      # 创建ROS2节点对象并进行初始化
    node.get_logger().info("ROS2节点示例：检测图像中的苹果")

    cap = cv2.VideoCapture(0)

    
    while rclpy.ok():
        ret, image = cap.read()          # 读取一帧图像
         
        if ret == True:
            object_detect(image)          # 苹果检测
        
    node.destroy_node()                  # 销毁节点对象
    rclpy.shutdown()                     # 关闭ROS2 Python接口

实现具体功能或结构还是需要py/c++语法支持的，后续还应该学习基本语法知识。
节点命令常见操作：

ros2 node list #查看节点列表
ros2 node info <node_name> #查看某节点信息

1.2话题：

话题使用发布订阅模型，消息就在节点之间流转，发布者和订阅者数量都灵活可变。
用.msg文件来定义发布/订阅数据结构。话题发布订阅过程可连续周期进行。

1.2.1创建话题发布者：

流程包括：

1.编程接口初始化
2.创建节点并初始化
3.创建发布者对象
4.创建并填充话题消息
5.发布话题消息
6.销毁节点并关闭接口

不要忘记配置程序入口与编译

import rclpy                                     # ROS2 Python接口库
from rclpy.node import Node                      # ROS2 节点类
from std_msgs.msg import String                  # 字符串消息类型

"""
创建一个发布者节点
"""
class PublisherNode(Node):
    
    def __init__(self, name):
        super().__init__(name)                                    # ROS2节点父类初始化
        self.pub = self.create_publisher(String, "chatter", 10)   # 创建发布者对象（消息类型、话题名、队列长度）
        self.timer = self.create_timer(0.5, self.timer_callback)  # 创建一个定时器（单位为秒的周期，定时执行的回调函数）
        
    def timer_callback(self):                                     # 创建定时器周期执行的回调函数
        msg = String()                                            # 创建一个String类型的消息对象
        msg.data = 'Hello World'                                  # 填充消息对象中的消息数据
        self.pub.publish(msg)                                     # 发布话题消息
        self.get_logger().info('Publishing: "%s"' % msg.data)     # 输出日志信息，提示已经完成话题发布
        
def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = PublisherNode("topic_helloworld_pub")     # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                 # 循环等待ROS2退出
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

用self.create_publisher()方法创建发布者对象。

1.2.2创建话题订阅者：

流程包括：

1.编程接口初始化
2.创建节点并初始化
3.创建订阅者对象
4.回调函数处理话题数据
5.销毁节点并关闭接口

import rclpy                                     # ROS2 Python接口库
from rclpy.node   import Node                    # ROS2 节点类
from std_msgs.msg import String                  # ROS2标准定义的String消息

"""
创建一个订阅者节点
"""
class SubscriberNode(Node):
    
    def __init__(self, name):
        super().__init__(name)                                    # ROS2节点父类初始化
        self.sub = self.create_subscription(\
            String, "chatter", self.listener_callback, 10)        # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）

    def listener_callback(self, msg):                             # 创建回调函数，执行收到话题消息后对数据的处理
        self.get_logger().info('I heard: "%s"' % msg.data)        # 输出日志信息，提示订阅收到的话题消息
        
def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = SubscriberNode("topic_helloworld_sub")    # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                 # 循环等待ROS2退出
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

1.3.服务：

与话题通讯不同，服务通讯更像是你问我答，先有请求再有反馈，一般不能连续周期进行。服务器唯一，客户端可以不唯一。用.srv文件来定义请求和应答数据结构。

1.3.1.创建服务客户端的程序流程：

1.编程接口初始化。
2.创建节点并初始化。
3.创建客户端对象。
4.创建并发送请求数据。
5.等待服务器端应答数据。
6.销毁节点并关闭接口。

ROS2服务示例-请求目标识别，等待目标位置应答：

import rclpy                                            # ROS2 Python接口库
from rclpy.node   import Node                           # ROS2 节点类
from learning_interface.srv import GetObjectPosition    # 自定义的服务接口

class objectClient(Node):
    def __init__(self, name):
        super().__init__(name)                                                                  # ROS2节点父类初始化
        self.client = self.create_client(GetObjectPosition, 'get_target_position')
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.request = GetObjectPosition.Request()
                    
    def send_request(self):
        self.request.get = True
        self.future = self.client.call_async(self.request)

def main(args=None):
    rclpy.init(args=args)                             # ROS2 Python接口初始化
    node = objectClient("service_object_client")       # 创建ROS2节点对象并进行初始化
    node.send_request()
    
    while rclpy.ok():
        rclpy.spin_once(node)

        if node.future.done():
            try:
                response = node.future.result()
            except Exception as e:
                node.get_logger().info(
                    'Service call failed %r' % (e,))
            else:
                node.get_logger().info(
                    'Result of object position:\n x: %d y: %d' %
                    (response.x, response.y))
            break
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

1.3.2.创建服务服务端的程序流程：

1.编程接口初始化。
2.创建节点并初始化。
3.创建服务端对象。
4.通过回调函数处进行服务
5.向客户端反馈应答结果。
6.销毁节点并关闭接口。

ROS2服务示例-提供目标识别服务：

import rclpy                                           # ROS2 Python接口库
from rclpy.node import Node                            # ROS2 节点类
from sensor_msgs.msg import Image                      # 图像消息类型
import numpy as np                                     # Python数值计算库
from cv_bridge import CvBridge                         # ROS与OpenCV图像转换类
import cv2                                             # Opencv图像处理库
from learning_interface.srv import GetObjectPosition   # 自定义的服务接口

lower_red = np.array([0, 90, 128])     # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255])  # 红色的HSV阈值上限

class ImageSubscriber(Node):
    def __init__(self, name):
        super().__init__(name)                                          # ROS2节点父类初始化
        self.sub = self.create_subscription(
            Image, 'image_raw', self.listener_callback, 10)             # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
        self.cv_bridge = CvBridge()                                     # 创建一个图像转换对象，用于OpenCV图像与ROS的图像消息的互相转换

        self.srv = self.create_service(GetObjectPosition,               # 创建服务器对象（接口类型、服务名、服务器回调函数）
                                       'get_target_position',
                                       self.object_position_callback)    
        self.objectX = 0
        self.objectY = 0                              

    def object_detect(self, image):
        hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)             # 图像从BGR颜色模型转换为HSV模型
        mask_red = cv2.inRange(hsv_img, lower_red, upper_red)        # 图像二值化
        contours, hierarchy = cv2.findContours(
            mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)          # 图像中轮廓检测

        for cnt in contours:                                         # 去除一些轮廓面积太小的噪声
            if cnt.shape[0] < 150:
                continue

            (x, y, w, h) = cv2.boundingRect(cnt)                     # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
            cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)       # 将苹果的轮廓勾勒出来
            cv2.circle(image, (int(x+w/2), int(y+h/2)), 5,
                       (0, 255, 0), -1)                              # 将苹果的图像中心点画出来
            
            self.objectX = int(x+w/2)
            self.objectY = int(y+h/2)

        cv2.imshow("object", image)                                  # 使用OpenCV显示处理后的图像效果
        cv2.waitKey(50)

    def listener_callback(self, data):
        self.get_logger().info('Receiving video frame')               # 输出日志信息，提示已进入回调函数
        image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8')            # 将ROS的图像消息转化成OpenCV图像
        self.object_detect(image)                                      # 苹果检测

    def object_position_callback(self, request, response):            # 创建回调函数，执行收到请求后对数据的处理
        if request.get == True:
            response.x = self.objectX                                 # 目标物体的XY坐标
            response.y = self.objectY
            self.get_logger().info('Object position\nx: %d y: %d' %
                                   (response.x, response.y))          # 输出日志信息，提示已经反馈
        else:
            response.x = 0
            response.y = 0
            self.get_logger().info('Invalid command')                 # 输出日志信息，提示已经反馈
        return response


def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = ImageSubscriber("service_object_server")  # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                 # 循环等待ROS2退出
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

1.4.通信接口：

接口就是一种相互关系，只有彼此匹配才能建立链接。ROS常用的通信接口：话题（topic）、服务（service）、动作（action），而进行通信所用的消息类型和三种接口的类型有关。

三种接口定义：

查看接口定义的命令：

ros2 interface (show) <...> #查看预定义接口详细信息（补不全则会现实有哪些）
ros2 interface package <功能包名> #查看功能包中的自定义接口有哪些

俗话说接口的定义，其实是实现通讯所需要的消息类型（数据结构）的定义。
案例：话题接口的定义与使用

话题通信接口的定义也是类似的，继续从之前的机器视觉案例中来衍生，我们想把服务换成话题，周期发布目标识别的位置，不管有没有人需要。

运行效果

现在我们会运行三个节点：
第一个节点，将驱动相机并发布图像话题，此时的话题数据使用的是ROS中标准定义的Image图像消息；
第二个节点，会运行视觉识别功能，识别目标的位置，这个位置我们希望封装成话题消息，发布出去，谁需要使用谁就来订阅；
第三个节点，订阅位置话题，打印到终端中。
启动三个终端，分别运行以上节点：

$ ros2 run usb_cam usb_cam_node_exe
$ ros2 run learning_topic interface_object_pub
$ ros2 run learning_topic interface_object_sub

定义接口

在这个例程中，我们使用ObjectPosition.msg定义了服务通信的接口：
需要在功能包外创建一个存放接口定义的功能包，在里面创建存放msg接口定义的文件夹，再创建：
（learning_interface/msg/）ObjectPosition.msg
编辑文件：

话题消息的内容是一个位置，我们使用x、y坐标值进行描述。

完成定义后，还需要在功能包的CMakeLists.txt中配置编译选项，让编译器在编译过程中，根据接口定义，自动生成不同语言的代码：

...
find_package(rosidl_default_generators REQUIRED)
  
rosidl_generate_interfaces(${PROJECT_NAME} 
    "msg/ObjectPosition.msg")
...

接口调用的时候注意from …import…地址正确，具体运用方法就替换消息类型即可。

到此，还应该掌握的知识：类与对象。

1.4.Rviz：

1.4.1.启动Rviz：

ros2 run rviz2 rviz2

1.4.2.主界面情况：

1.上部为工具栏：包括视角控制、预估位姿设置、目标设置等，还可以添加自定义插件；
2.左侧为插件显示区：包括添加、删除、复制、重命名插件，显示插件，以及设置插件属性等功能；
3.中间为3D试图显示区：以可视化的方式显示添加的插件信息；
4.右侧为观测视角设置区：可以设置不同的观测视角；
5.下侧为时间显示区：包括系统时间和ROS时间。

左侧插件显示区默认有两个插件：

Global Options：该插件用于设置全局显示相关的参数，一般情况下，需要自行设置的是 Fixed Frame 选项，该选项是其他所有数据在可视化显示时所参考的全局坐标系；
Global Status：该插件用于显示在 Global Options 设置完毕 Fixed Frame 之后，所有的坐标变换是否正常。

右侧观测视角设置区：

有Orbit、ThirdPersonFollower等视角类型可供选择。

1.4.3.rviz2 中的预定义插件：

名称	功能	消息类型
Axes	显示 rviz2 默认的坐标系。
Camera	显示相机图像，必须需要使用消息：CameraInfo。	sensormsgs/msg/Image，sensormsgs/msg/CameraInfo
Grid	显示以参考坐标系原点为中心的网格。
Grid Cells	从网格中绘制单元格，通常是导航堆栈中成本地图中的障碍物。	navmsgs/msg/GridCells
Image	显示相机图像，但是和Camera插件不同，它不需要使用 CameraInfo 消息。	sensormsgs/msg/Image
InteractiveMarker	显示来自一个或多个交互式标记服务器的 3D 对象，并允许与它们进行鼠标交互。	visualizationmsgs/msg/InteractiveMarker
Laser Scan	显示激光雷达数据。	sensormsgs/msg/LaserScan
Map	显示地图数据。	navmsgs/msg/OccupancyGrid
Markers	允许开发者通过主题显示任意原始形状的几何体。	visualizationmsgs/msg/Marker，visualizationmsgs/msg/MarkerArray
Path	显示机器人导航中的路径相关数据。	navmsgs/msg/Path
PointStamped	以小球的形式绘制一个点。	geometrymsgs/msg/PointStamped
Pose	以箭头或坐标轴的方式绘制位姿。	geometrymsgs/msg/PoseStamped
Pose Array	绘制一组 Pose。	geometrymsgs/msg/PoseArray
Point Cloud2	绘制点云数据。	sensormsgs/msg/PointCloud，sensormsgs/msg/PointCloud2
Polygon	将多边形的轮廓绘制为线。	geometrymsgs/msg/Polygon
Odometry	显示随着时间推移累积的里程计消息。	navmsgs/msg/Odometry
Range	显示表示来自声纳或红外距离传感器的距离测量值的圆锥。	sensormsgs/msg/Range
RobotModel	显示机器人模型。
TF	显示 tf 变换层次结构。
Wrench	将geometrymsgs /WrenchStamped消息显示为表示力的箭头和表示扭矩的箭头加圆圈。	geometrymsgs/msg/WrenchStamped
Oculus	将 RViz 场景渲染到 Oculus 头戴设备。

1.5.python基础：

由于篇幅限制，已于1月23日上传第一部分：Python基础（1）https://editor.csdn.net/md/?articleId=135776769