ROS2-DL 基于Ubuntu20.04—通过ros2发布自己的deep learning检测结果(三)(根据检测结果自定义msg)

一、创建自定义msg消息

准备：检测输出消息及格式确定

在创建自定义msg之前，确定yolov4dm检测输出内容需要包括车位框坐标信息、障碍物框坐标信息 并都各自整体以列表传递。
修改YOLOV4DM/src/yolov4dm/yolov4dm/yolo.py
车位框坐标信息：


障碍物框坐标信息：

返回

修改YOLOV4DM/src/yolov4dm/yolov4dm/yolo.py

1.创建yolov4dm_message消息功能包

从my_message(这是我之前专门放各种自定义msg位置，为了方便以后SLAM接受检测数据，所以就不专门在YOLOV4DM工作空间中创建自定义msg)进入终端：

ros2 pkg create --build-type ament_cmake yolov4dm_message

2.创建.msg

在yolov4dm_message中创建msg文件夹 并在msg中创建Parkpose.msg文件

Parkpose.msg内容：

std_msgs/Header header  #这是引用std_msgs的时间辍格式
float64[] obstacle
float64[] parkspace

3.修改CMakeLists.txt

修改yolov4dm_message中CMakeLists.txt，增加：

find_package(std_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Parkpose.msg"
  DEPENDENCIES std_msgs
 )

4.修改package.xml

修改yolov4dm_message中package.xml，增加：

<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

5.编译

从my_message进入终端：

colcon build --packages-select yolov4dm_message

6.环境配置

在主目录按ctrl+H，双击打开.bashrc文件，添加：

source /home/stk/ros2work/my_message/install/setup.bash

保存并刷新：source ~/.bashrc

7.验证

终端输入：

ros2 interface show yolov4dm_message/msg/Parkpose.msg

注意：如果这里验证成功，却在后来使用自定义msg时查看相关话题发布内容 报错说没有该模块，可尝试以下方法：
1）查看你的环境配置路径是否与你的路径一致
2）有时候新环境不会立即生效，尝试重启终端或重启电脑

二、修改YOLOV4DM功能包

在上一篇中的YOLOV4DM功能包基础上修改。

1.添加依赖项yolov4dm_message

修改package.xml，添加：

<depend>yolov4dm_message</depend>

修改predict.py，添加：

from yolov4dm_message.msg import Parkpose

2.修改发布话题

predict.py：
1）引用

from rclpy.clock import Clock

2）修改

self.publisher_ = self.create_publisher(Parkpose, 'point_topic', 10)

3）话题发布内容

 msgss = Parkpose()      
 msgss.header.stamp = Clock().now().to_msg()
 msgss.header.frame_id = "park_link"
 msgss.obstacle = obstacle_list
 msgss.parkspace = park_list
 self.publisher_.publish(msgss)

程序：

import sys
sys.path.append('/home/stk/ros2work/ORB_SLAM3_Grid_Mapping/Examples/YOLOV4DM/src/yolov4dm/yolov4dm')
import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
import cv2
import numpy as np
from matplotlib import pyplot as plt
from PIL import Image as Images
from cv_bridge import CvBridge
from yolo import YOLO
from geometry_msgs.msg import Point
from sensor_msgs.msg import Image
from yolov4dm_message.msg import Parkpose

class ImageSubscriber(Node):
    def __init__(self):
        super().__init__('image_subscriber')
        super().__init__('point_publisher')
        self.subscription = self.create_subscription(
            Image,
            '/image_topic',
            self.image_callback,
            10)
        self.cv_bridge = CvBridge()     
        self.publisher_ = self.create_publisher(Parkpose, 'point_topic', 10)  
 
    def image_callback(self, msg):
        cv2_img = self.cv_bridge.imgmsg_to_cv2(msg,'bgr8')
        self.get_logger().info('Received image with dimensions: %s' % str(cv2_img.shape))
        msgss = Parkpose()
        obstacle_list = []
        park_list = []    
        yolo = YOLO()
        mode = "predict"
        if mode == "predict":              
            try:
                image = Images.fromarray(cv2.cvtColor(cv2_img,cv2.COLOR_BGR2RGB))
                img_read = cv2_img
            except:
                print('Open Error! Try again!')
            else:
                r_image,obstacle_list,park_list = yolo.detect_image(image,img_read, crop = False, count=False)   
                print(obstacle_list)
                print(park_list)
                 
        msgss.header.stamp = Clock().now().to_msg()
        msgss.header.frame_id = "park_link"
        msgss.obstacle = obstacle_list
        msgss.parkspace = park_list
        self.publisher_.publish(msgss)
 
def main(args=None):
    rclpy.init(args=args)
    image_subscriber = ImageSubscriber()
    rclpy.spin(image_subscriber)
    rclpy.shutdown()
 
if __name__ == '__main__':
    main()

3.编译

YOLOV4DM进入终端：colcon build

三、运行

第一个终端，从SimImage进入(上一篇文章详细写了如何创建运行SimImage功能包)：

. install/setup.bash
ros2 run simimage imagetopic

第二个终端，从YOLOV4DM进入：

. install/setup.bash
ros2 run yolov4dm predict

第三个终端，随便从哪儿进入：

ros2 topic echo /point_topic