Understanding the OriginBot Smart Robot Project: Camera Driver Source Code Analysis

This blog post mainly records my learning and understanding of the OriginBot camera driver and visualization (http://originbot.org/manual/camera_visualization/#ros) code, and I will annotate it in the code files.

The documentation (http://originbot.org/manual/camera_visualization/#_2) provides two methods for driving the camera: one method allows real-time display of images and results of human detection algorithms (https://developer.horizon.ai/documents_tros/boxs/function/mono2d_body_detection) through a webpage after starting, while the other method only publishes image data through a topic after starting.

ros2 launch originbot_bringup camera_websoket_display.launch.py

After starting, open http://IP:8000 in a browser.

The final executed code for this command is originbot.originbot_bringup.launch.camera_websoket_display.launch.py, and the specific content is as follows:

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python import get_package_share_directory
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    mipi_cam_device_arg = DeclareLaunchArgument(
        'device',
        default_value='GC4663',
        description='mipi camera device')
    # This is the Node that actually starts the camera, and the final execution is mipi_cam.launch.py, which will be explained separately below
    mipi_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('mipi_cam'),
                'launch/mipi_cam.launch.py')),
        launch_arguments={
            'mipi_image_width': '960',
            'mipi_image_height': '544',
            'mipi_io_method': 'shared_mem',
            'mipi_video_device': LaunchConfiguration('device')
        }.items()
    )
    # nv12->jpeg
    # Here we call the image codec module of TogetheROS.Bot to improve performance, for details refer to:
    # https://developer.horizon.cc/documents_tros/quick_demo/hobot_codec
    jpeg_codec_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('hobot_codec'),
                'launch/hobot_codec_encode.launch.py')),
        launch_arguments={
            'codec_in_mode': 'shared_mem',
            'codec_out_mode': 'ros',
            'codec_sub_topic': '/hbmem_img',
            'codec_pub_topic': '/image'
        }.items()
    )
    # web
    # This is the part that starts the web, in fact, behind it is an Nginx static server,
    # which subscribes to images to display pictures and subscribes to smart_topic to obtain human detection data.
    # Finally, it executes the websocket.laucn.py code, which will be explained in detail later.
    web_smart_topic_arg = DeclareLaunchArgument(
        'smart_topic',
        default_value='/hobot_mono2d_body_detection',
        description='websocket smart topic')
    web_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('websocket'),
                'launch/websocket.launch.py')),
        launch_arguments={
            'websocket_image_topic': '/image',
            'websocket_smart_topic': LaunchConfiguration('smart_topic')
        }.items()
    )
    # mono2d body detection
    # TogetheROS.Bot's human detection function,
    # will subscribe to /image_raw or /hbmem_img image data for detection,
    # then publish the detection results to hobot_mono2d_body_detection,
    # I have used this module in https://www.guyuehome.com/45835, which also has a relatively detailed introduction, please refer to
    # Source code and official documentation at: https://developer.horizon.cc/documents_tros/quick_demo/hobot_codec
    mono2d_body_pub_topic_arg = DeclareLaunchArgument(
        'mono2d_body_pub_topic',
        default_value='/hobot_mono2d_body_detection',
        description='mono2d body ai message publish topic')
    mono2d_body_det_node = Node(
        package='mono2d_body_detection',
        executable='mono2d_body_detection',
        output='screen',
        parameters=[
            {"ai_msg_pub_topic_name": LaunchConfiguration(
                'mono2d_body_pub_topic')}
        ],
        arguments=['--ros-args', '--log-level', 'warn']
    )
    return LaunchDescription([
        mipi_cam_device_arg,
        # image publish
        mipi_node,
        # image codec
        jpeg_codec_node,
        # body detection
        mono2d_body_pub_topic_arg,
        mono2d_body_det_node,
        # web display
        web_smart_topic_arg,
        web_node
    ])

The above code calls mipi_cam.launch.py and websocket.launch.py, which will be introduced separately.

Below is the content of originbot.mipi_cam.launch.mipi_cam.launch.py:

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument(
            'mipi_camera_calibration_file_path',
            default_value='/userdata/dev_ws/src/origineye/mipi_cam/config/SC132GS_calibration.yaml',
            description='mipi camera calibration file path'),
        DeclareLaunchArgument(
            'mipi_out_format',
            default_value='nv12',
            description='mipi camera out format'),
        DeclareLaunchArgument(
            'mipi_image_width',
            default_value='1088',
            description='mipi camera out image width'),
        DeclareLaunchArgument(
            'mipi_image_height',
            default_value='1280',
            description='mipi camera out image height'),
        DeclareLaunchArgument(
            'mipi_io_method',
            default_value='shared_mem',
            description='mipi camera out io_method'),
        DeclareLaunchArgument(
            'mipi_video_device',
            default_value='F37',
            description='mipi camera device'),
        # Start the image publishing package
        Node(
            package='mipi_cam',
            executable='mipi_cam',
            output='screen',
            parameters=[
                {"camera_calibration_file_path": LaunchConfiguration(
                    'mipi_camera_calibration_file_path')},
                {"out_format": LaunchConfiguration('mipi_out_format')},
                {"image_width": LaunchConfiguration('mipi_image_width')},
                {"image_height": LaunchConfiguration('mipi_image_height')},
                {"io_method": LaunchConfiguration('mipi_io_method')},
                {"video_device": LaunchConfiguration('mipi_video_device')},
                {"rotate_degree": 90},
            ],
            arguments=['--ros-args', '--log-level', 'error']
        )
    ])

This part of the code is quite simple, just some parameter declarations, but if you have used OriginBot for a while, you should remember that after starting the camera, the car will publish image data through a topic called /image_raw, which is not mentioned here.

This part is in originbot.mipi_cam.src.mipi_cam_node.cpp line 236, the function is as follows:

  if (io_method_name_.compare("ros") == 0) {
    image_pub_ =      this->create_publisher<sensor_msgs::msg::Image>("image_raw", PUB_BUF_NUM);
  }

import os
import subprocess
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_prefix

def generate_launch_description():
    # Start the webserver service
    name = 'nginx'
    nginx = "./sbin/" + name
    webserver = nginx + " -p ."
    launch_webserver = True
    # Query the process list to get all processes containing the webserver string
    processes = subprocess.check_output(['ps', 'ax'], universal_newlines=True)
    processes = [p.strip() for p in processes.split('\n') if webserver in p]
    # If there are processes, it means the target program is already running
    if len(processes) > 0:
        launch_webserver = False
    if launch_webserver:
        print("launch webserver")
        pwd_path = os.getcwd()
        print("pwd_path is ", pwd_path)
        webserver_path = os.path.join(get_package_prefix('websocket'),
                                      "lib/websocket/webservice")
        print("webserver_path is ", webserver_path)
        os.chdir(webserver_path)
        # os.chmod(nginx, stat.S_IRWXU)
        print("launch webserver cmd is ", webserver)
        os.system(webserver)
        os.chdir(pwd_path)
    else:
        print("webserver has launch")
    return LaunchDescription([
        DeclareLaunchArgument(
            'websocket_image_topic',
            default_value='/image_jpeg',
            description='image subscribe topic name'),
        DeclareLaunchArgument(
            'websocket_image_type',
            default_value='mjpeg',
            description='image type'),
        DeclareLaunchArgument(
            'websocket_only_show_image',
            default_value='False',
            description='only show image'),
        DeclareLaunchArgument(
            'websocket_output_fps',
            default_value='0',
            description='output fps'),
        DeclareLaunchArgument(
            'websocket_smart_topic',
            default_value='/hobot_mono2d_body_detection',
            description='smart message subscribe topic name'),
        Node(
            package='websocket',
            executable='websocket',
            output='screen',
            parameters=[
                {"image_topic": LaunchConfiguration('websocket_image_topic')},
                {"image_type": LaunchConfiguration('websocket_image_type')},
                {"only_show_image": LaunchConfiguration(
                    'websocket_only_show_image')},
                {"output_fps": LaunchConfiguration('websocket_output_fps')},
                {"smart_topic": LaunchConfiguration('websocket_smart_topic')}
            ],
            arguments=['--ros-args', '--log-level', 'error']
        )
    ])

As you can see, this launch file actually starts an nginx process and a websocket Node. The source code of this websocket can be found here (https://github.com/HorizonRDK/hobot_websocket), but I feel that general developers don’t need to delve into it. As long as it meets their needs, they can use it directly, as this is the purpose of Horizon’s development of TogetheROS.Bot.

ros2 launch originbot_bringup camera.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='mipi_cam',
            executable='mipi_cam',
            output='screen',
            parameters=[
                {"mipi_camera_calibration_file_path": "/opt/tros/lib/mipi_cam/config/GC4663_calibration.yaml"},
                {"out_format": "bgr8"},
                {"image_width": 960},
                {"image_height": 544},
                {"io_method": "ros"},
                {"mipi_video_device": "GC4663"}
            ],
            arguments=['--ros-args', '--log-level', 'error']
        ),
        Node(
            package='originbot_demo',
            executable='transport_img',
            arguments=['--ros-args', '--log-level', 'error']
        ),
    ])

import cv2
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge

class ImageCompressor(Node):
    def __init__(self):
        super().__init__('image_compressor')
        self.bridge = CvBridge()
        self.image_sub = self.create_subscription(Image, 'image_raw', self.callback, 10)
        self.compressed_pub = self.create_publisher(CompressedImage, 'compressed_image', 10)
        self.bgr8_pub = self.create_publisher(Image, 'bgr8_image', 10)
    def callback(self, msg):
        cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
        encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 50]
        _, compressed = cv2.imencode('.jpg', cv_image, encode_param)
        compressed_msg = CompressedImage()
        compressed_msg.header = msg.header
        compressed_msg.format = 'jpeg'
        compressed_msg.data = np.array(compressed).tostring()
        self.compressed_pub.publish(compressed_msg)
        decompressed = cv2.imdecode(np.frombuffer(compressed_msg.data, np.uint8), cv2.IMREAD_COLOR)
        bgr8_msg = self.bridge.cv2_to_imgmsg(decompressed, encoding='bgr8')
        self.compressed_pub.publish(compressed_msg)
        self.bgr8_pub.publish(bgr8_msg)

def main(args=None):
    rclpy.init(args=args)
    compressor = ImageCompressor()
    rclpy.spin(compressor)
    rclpy.shutdown()

if __name__ == '__main__':
    main()