亚博microros小车-原生ubuntu支持系列：2-摄像头控制

本篇继续迁移亚博microros小车的图传模块。

一  图传代理启动

启动脚本：

docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 9999 -v4

小车连上后

二 显示图像代码

src/yahboom_esp32_camera/yahboom_esp32_camera/目录下新建文件sub_img.py

代码如下：

import rclpy
import time
from rclpy.node import Node
import cv2 as cv
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, CompressedImage

from rclpy.time import Time
import datetime


class SubImg(Node):
    def __init__(self, name):
        super().__init__(name)
        self.bridge = CvBridge()
        #订阅图传模块图像话题，其中hanleTopic是回调函数
        self.sub_img = self.create_subscription(
            CompressedImage, '/espRos/esp32camera', self.handleTopic, 1)
        self.pub_img = self.create_publisher(Image, "/esp32_img", 1)

        self.last_stamp = None
        self.new_seconds = 0
        self.fps_seconds = 1

    def handleTopic(self, msg):
        self.last_stamp = msg.header.stamp  
        if self.last_stamp:#时间处理
            total_secs = Time(nanoseconds=self.last_stamp.nanosec, seconds=self.last_stamp.sec).nanoseconds
            delta = datetime.timedelta(seconds=total_secs * 1e-9)
            seconds = delta.total_seconds()*100

            if self.new_seconds != 0:
                self.fps_seconds = seconds - self.new_seconds

            self.new_seconds = seconds#保留这次的值

            #print(self.fps_seconds)
            
        start = time.time()
      
        #图像类型转换
        frame = self.bridge.compressed_imgmsg_to_cv2(msg)
        # print(frame)
        frame = cv.resize(frame, (640, 480))
        cv.waitKey(10)
        end = time.time()
        #print(self.fps_seconds)
        fps = 1/((end - start)+self.fps_seconds) 
        text = "FPS : " + str(int(fps))
        cv.putText(frame, text, (30, 30), cv.FONT_HERSHEY_SIMPLEX,
                   0.6, (100, 200, 200), 1)#显示fps
        # msg = self.bridge.cv2_to_imgmsg(frame, "bgr8")

        cv.imshow("color_image", frame)
        img_msg = self.bridge.cv2_to_imgmsg(frame, 'bgr8')#把图像解析为bgr8格式，也是opencv的默认图像格式
        self.pub_img.publish(img_msg)#发布图像话题
        cv.waitKey(10)


def main():
    rclpy.init()
    esp_img = SubImg("sub_img")
    try:
        rclpy.spin(esp_img)
    except KeyboardInterrupt:
        pass
    finally:
        esp_img.destroy_node()
        rclpy.shutdown()

主要逻辑就是订阅摄像头的话题，用opencv做图像转换并再次发布图像。

构建后运行 : ros2 run yahboom_esp32_camera sub_img

看下这个节点信息

bohu@bohu-TM1701:~$ ros2 node info /espRos/Esp32Node
/espRos/Esp32Node
  Subscribers:

  Publishers:
    /espRos/esp32camera: sensor_msgs/msg/CompressedImage
  Service Servers:

  Service Clients:

  Action Servers:

  Action Clients:

它就是提供了压缩图像的发布话题。接口是:sensor_msgs/msg/CompressedImage

 处理消息的格式

三 摄像头画面倒置

调用亚博官方的调整脚本， SET_Camera.py

import socket
import time

staip = "192.168.2.93" #填写开启dotcker后信息的ip地址
PORT = 8888

vflip_OFF = "@vflip:0@"     #不倒转 
vflip_OPEN = "@vflip:1@"    #倒转
mirror_OFF = "@mirror:0@"   #不开启水平镜像
mirror_OPEN = "@mirror:1@"  #开启水平镜像


 



def set_Camera(vflip_flag,mirror_flag):
    if vflip_flag == True:
        send_data = vflip_OPEN
        sk.sendall(bytes(send_data, encoding="utf8"))
    elif vflip_flag == False:
        send_data = vflip_OFF
        sk.sendall(bytes(send_data, encoding="utf8"))


    time.sleep(1)
    if mirror_flag == True:
        send_data = mirror_OPEN
        sk.sendall(bytes(send_data, encoding="utf8"))
    elif mirror_flag == False:
        send_data = mirror_OFF
        sk.sendall(bytes(send_data, encoding="utf8"))


print("please input docket ipV4:")
staip = input()

sk = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# 连接服务器 socket tcp
try:
    sk.connect((staip,PORT))   
    set_Camera(True,True) #反转画面
    #set_Camera(False,False) #不反转画面
    print("Camera is set ok!")
    sk.close()
except KeyboardInterrupt:
    sk.close()
except Exception as e:
    print("Camera is set fail!")
    print("Program Error:", Exception)
    sk.close()
    

注意传的IP不是小车的IP，是启动图传代理之后链接的docker IP