ROS节点: 图片与 numpy array订阅器与发布器 (python)

---

在做项目的时候需要写到ros节点, 其实这是一个很格式化的活, 所以记录一下, 方便以后要是用到了可以直接复制粘贴.
Life is difficult, make it easy and brainless.

---

一些bb

我感觉 Ros 中不支持直接传递ndarray的消息类型, 所以一般有下面几种办法来传,

1. 用cvBridge包里的函数把ndarray消息变成sensor_msg里的Image, 然后让C++订阅器用同样的消息类型和cvBridge函数得到的一个Image Mat数据了.
2. 发布器把二维数组reshape成1维数组, C++订阅器接受到了1维数组后自行实现数组上的reshape成多维
3. 把消息打包成一个装有自定义结构体的一维数组

图片发布器

第一种形式, 直接贴一下自己写的水代码, 供参考:

video_Image_talker.py

# encoding = utf-8                        # 不加这一句的话注释写中文就会报错
#!/usr/bin/env python2                    # 指定自己要的python2解释器
"""
/***************************************
@Copyright(C) All rights reserved.
@Author KUO SU
@Date   2020.11
***************************************/
"""


import time
import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
import copy
from datetime import datetime
import glog
import os

# ros 基础功能和消息常用包(正常安装了ros都会有的)
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError


class VideoImageTalker(object):

    def __init__(self, video_path, msg_type=Image, topic_name='talk video frame', rate=5):
        self.publisher = rospy.Publisher(topic_name, msg_type, queue_size=1)
        try:
            self.video_path = os.path.abspath(video_path)
        except RuntimeError as e:
            glog.info("the video path is invalid, {}".format(e))
        try:
            self.rate = rospy.Rate(rate)
        except RuntimeError as e:
            glog.info("the rate is invalid, {}".format(e))

    def set_publisher(self, video_path, msg_type=Image, topic_name='talk video frame'):
        self.publisher = rospy.Publisher(topic_name, msg_type, queue_size=1)
        try:
            self.video_path = os.path.abspath(video_path)
        except RuntimeError as e:
            glog.info("the video path is invalid, {}".format(e))

    def set_rate(self, rate):
        self.rate = rospy.Rate(rate)

    def publish_video_frame(self, rate=10, node_name="video talker"):
        try:
            rospy.init_node('image_node')
            video_capture = cv2.VideoCapture(self.video_path)
            while True:
                ret, frame = video_capture.read()  # frame shape 640*480*3
                if not ret:
                    break
                while not rospy.is_shutdown():
                    rospy.loginfo("talking a frame")
                    self.publisher.publish(frame)
                    self.rate.sleep()
        except rospy.ROSInterruptException:
            rospy.logerr('somthing run in fun: publish video frame')

图片订阅器

image_subsriber.py

#!/usr/bin/env python2
#encoding = utf-8
"""
/***************************************
@Copyright(C) All rights reserved.
@Author KUO SU
@Date   2020.11
***************************************/
"""

import time
import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
import copy
from datetime import datetime

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from Track.msg import Tracks


class ImageSubscriber:

    def __init__(self, topic_name, msg_type=Image):
        self.bridge = CvBridge()
        try:
            self.subscriber = rospy.Subscriber(topic_name, msg_type, self.call_back)
            self.raw_image = np.full((20,20,3), 255)
        except RuntimeError as e:
            raise Exception("cant create subscriber, {}".format(e))

    def set_subscriber(self, topic_name, msg_type):
        try:
            self.subscriber = rospy.Subscriber(msg_type, topic_name, self.call_back)
        except RuntimeError as e:
            raise Exception("cant create subscriber, {}".format(e))

    def call_back(self, data):

        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            rospy.loginfo("got an Image: {}".format(type(cv_image)))
            self.raw_image = cv_image.copy()
        except CvBridgeError as e:
            print(e)
        rospy.loginfo("image: {}".format(type(self.raw_image)))

    def get_image(self):
        return self.raw_image

if __name__ == "__main__":
    image_subscriber = ImageSubscriber(topic_name="image_subsriber")
    image = image_subscriber.get_image()
    print(image)

自定义ndarray消息(如 BoundingBox)

用的第三种形式:

这里贴的是我一个多目标跟踪项目的代码, 期望发布的是 BoundingBox, 和对应的 id号

所以消息类型用的 boxes是一个一维数组, 里面装box结构体实例, box结构体里面定义box的左上和右下角坐标, 至于id, 那就是装了int 类型的一维数组

msg file

贴上对应的msg file

Box.msg

int32 x1
int32 y1
int32 x2
int32 y2

Tracks.msg

Header header   # 这里注意Header必须加
Box[] boxes
uint32[] ids

track_talker.py

"""
/***************************************
@Copyright(C) All rights reserved.
@Author KUO SU
@Date   2020.11
***************************************/
"""

import time
import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
import copy
from datetime import datetime

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from Track.msg import Tracks, Box

def to_box(box_list):
    box = Box()
    box.x1 = int(box_list[0])
    box.y1 = int(box_list[1])
    box.x2 = int(box_list[2])
    box.y2 = int(box_list[3])
    return box

class TrackPublisher(object):

    def __init__(self, topic_name, msg_type=Tracks, rate=5, queue_size = 1):
        self.publisher = rospy.Publisher(topic_name, msg_type, queue_size= queue_size)
        self.rate = rospy.Rate(5)
        self.tracks = Tracks()
        self.bridge = CvBridge()
        

    def set_publisher(self, custom_publisher):
        assert isinstance(self.publisher, custom_publisher), "parameter has to be am object of rospy Publisher"
        self.publisher = custom_publisher

    def set_rate(self, rate):
        self.rate = rospy.Rate(rate)

    def publish(self, boxes, ids):
        """
        @TODO: will use the data and publish the recieving data immediatly
               but now this is a demo test node
        """
        # while not rospy.is_shutdown():
        # boxes = self.bridge.cv2_to_imgmsg(boxes.astype(np.int32),'bgr8')        
        self.tracks.boxes = boxes
        self.tracks.ids = ids
        rospy.loginfo("publishing: {} \n{}".format(type(boxes), type(ids)))
        self.publisher.publish(self.tracks)
        # self.rate.sleep()



if __name__ == "__main__":
    rospy.init_node('track_publisher')
    tmp_publisher = TrackPublisher(topic_name= 'haha', msg_type=tracks)
    fack_bbx = [np.array([1,2,3,4]) for _ in range(6)]
    fack_ids = [i for i in range(6)]
    #try:
    while not rospy.is_shutdown():
        boxes = []
        indexs = []
        for box, ids in zip(fack_bbx, fack_ids):
            boxes.append(to_box(box.tolist()))
            indexs.append(ids)
        tmp_publisher.publish(boxes, indexs)
        rate.sleep()
    rospy.spin()
    #except: rospy.loginfo("Error: somthing wrong here")

track_subsriber.py

"""
/***************************************
@Copyright(C) All rights reserved.
@Author KUO SU
@Date   2020.11
***************************************/
"""

import rospy
import cv2
# from std_msgs.msg import Header
# from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from ros_patterns.image_subsriber import image_subscriber
from datetime import datetime
from test_tools.visualization import draw_tracks
from test_tools.display import set_display, open_window
from Track.msg import Tracks, Box


class TrackSubscriber:

    def __init__(self, msg_type, topic_name):

        self.bridge = CvBridge()
        self.gotten_track = Tracks()
        try:
            self.subscriber = rospy.Subscriber(msg_type, topic_name, self.call_back)
        except:
            raise Exception("cant create subscriber")

    def set_subscriber(self):

        pass

    def call_back(self, data):

        try:
            self.gotten_track = data.copy()
            rospy.loginfo("I am getting an image at {}".format(datetime.now()))
        except CvBridgeError as e:
            print(e)
        rospy.loginfo("image: {}".format(type(self.gotten_track)))

    def get_track(self):

        return self.gotten_track


class ShowResultNode(object):

    def __init__(self, msg_type, topic_name, track_type, track_topic):
        self.image_subsriber = image_subscriber(msg_type, topic_name)
        self.track_subsriber = TrackSubscriber(track_type, track_topic)
        self.visualizer = draw_tracks

    def show_vedio_track_result(self):
        frame = self.image_subsriber.get_image()
        track_data = self.track_subsriber.get_track()
        boxes, ids = track_data.boxes, track_data.ids
        frame = self.visualizer(frame, boxes, ids, 200)
        open_window("ROS NODE DEBUG show", "yolov3_with_deepsort_local_debug", 1024, 768)
        set_display("ROS NODE DEBUG show", False)
        cv2.imshow("ROS NODE DEBUG show", frame)
        if cv2.waitKey(100) & 0xFF == ord('q'):
            return 0


if __name__ == "__main__":
    pass

Cmakelist.txt

cmake_minimum_required(VERSION 3.0.2)
project(Track)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
  message_generation
  message_runtime
)


add_message_files(
   FILES
   Tracks.msg
   Box.msg
)


generate_messages(
   DEPENDENCIES
   std_msgs
   sensor_msgs
)


catkin_package(
    INCLUDE_DIRS include
#   LIBRARIES Traffic_Track
    CATKIN_DEPENDS roscpp rospy message_runtime std_msgs sensor_msgs
#   DEPENDS system_lib
)


include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

XML file

<?xml version="1.0"?>
<package format="2">
  <name>Track</name>
  <version>0.0.0</version>
  <description>The Traffic_Track package</description>

  <maintainer email="root@todo.todo">root</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>python-numpy</build_depend>
  <build_depend>message_runtime</build_depend>

  <build_export_depend>python-numpy</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>message_runtime</build_export_depend>

  <exec_depend>python-numpy</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>

  <export>

  </export>
</package>

仅供参考

祝你武运昌隆