跳转至

基础功能编程

获取IMU信息

运行例程

开启OriginMan电源后，可以在MoboXterm或者Vscode中启动如下指令：

ros2 run originman_demo imu_print_node
很快就可以在终端中看到IMU的内容发布：

image-20220923124715971

代码实现

实现该功能的代码文件是originman_demo/imu_print_node.py，详细的实现过程如下：

#!/usr/bin/env python3
import sys
import os
# 将当前脚本所在目录添加到 sys.path
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

import rclpy
from rclpy.node import Node
import time
from ros_robot_controller_sdk import Board


class IMUDisplayNode(Node):
    def __init__(self):
        super().__init__('imu_print_node')
        self.board = Board(device="/dev/ttyS1", baudrate=1000000) 
        self.board.enable_reception()
        self.get_logger().info("Board initialized")
        time.sleep(1)  # 等待1秒
        self.timer = self.create_timer(0.1, self.display_imu_callback)  # 每0.1秒读取一次

    def display_imu_callback(self):
        imu_data = self.board.get_imu()
        if imu_data is not None:
            ax, ay, az, gx, gy, gz = imu_data
            self.get_logger().info(f"IMU - Accel: ({ax:.2f}, {ay:.2f}, {az:.2f}) g, Gyro: ({gx:.2f}, {gy:.2f}, {gz:.2f}) deg/s")


def main(args=None):
    rclpy.init(args=args)
    imu_print_node = IMUDisplayNode()
    rclpy.spin(imu_print_node)
    imu_print_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

图像发布

运行例程

开启OriginMan电源后，可以在MoboXterm或者Vscode中启动如下指令：

ros2 run originman_demo image_display_node

很快就可以在终端中看到机器人发布图像的输出信息：

image-20220923124819442

代码实现

实现该功能的代码文件是originman_demo/image_display_node.py，详细的实现过程如下：

#!/usr/bin/env python3
# encoding:utf-8
import rclpy
from rclpy.node import Node
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image


class ImagePublisherNode(Node):
    def __init__(self):
        super().__init__('image_publisher_node')
        self.bridge = CvBridge()
        # 创建发布者，发布 image_raw 话题
        self.publisher_ = self.create_publisher(Image, 'image_raw', 10)
        # 打开摄像头，0 表示默认摄像头
        self.cap = cv2.VideoCapture(0)
        if not self.cap.isOpened():
            self.get_logger().error("无法打开摄像头")
        else:
            # 创建定时器，定期发布图像
            self.timer = self.create_timer(0.1, self.publish_image)

    def publish_image(self):
        ret, frame = self.cap.read()
        if ret:
            try:
                # 将 OpenCV 图像转换为 ROS 图像消息
                ros_image = self.bridge.cv2_to_imgmsg(frame, "bgr8")
                # 发布图像消息
                self.publisher_.publish(ros_image)
                self.get_logger().info('发布图像')
            except Exception as e:
                self.get_logger().error(f"Error converting image: {str(e)}")
        else:
            self.get_logger().error("无法读取图像帧")

    def __del__(self):
        # 释放摄像头资源
        if self.cap.isOpened():
            self.cap.release()


def main(args=None):
    rclpy.init(args=args)
    image_publisher_node = ImagePublisherNode()
    rclpy.spin(image_publisher_node)
    image_publisher_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

控制oled

运行例程

开启OriginMan电源后，可以在MoboXterm或者Vscode中启动如下指令：

ros2 run originman_demo oled_display_node

运行成功后，可以看到OriginMan背部的oled显示屏将会显示Wlan0的IP地址。

image-20220923124819442

代码实现

实现该功能的代码文件是originman_demo/oled_display_node.py，详细的实现过程如下：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import time
import numpy as np
import Adafruit_SSD1306
from PIL import Image, ImageDraw, ImageFont
import socket
import subprocess


class OledDisplayNode(Node):
    def __init__(self):
        super().__init__('oled_display_node')
        # 初始化OLED显示屏
        self.screen = Adafruit_SSD1306.SSD1306_128_64(rst=None, i2c_bus=5, gpio=1, i2c_address=0x3C)
        self.screen.begin()  # 启动屏幕
        self.screen.image(Image.fromarray(np.zeros((64, 128), dtype=np.uint8)).convert('1'))
        self.screen.display()

        # 加载字体
        self.font = ImageFont.load_default()

        # 创建定时器，每秒更新一次显示内容
        self.timer = self.create_timer(1.0, self.update_display)

    def get_wlan0_ip(self):
        """获取wlan0的IP地址，如果未连接则返回None"""
        try:
            # 使用ifconfig获取wlan0的IP地址
            result = subprocess.check_output(["ifconfig", "wlan0"], text=True)
            for line in result.split('/n'):
                if "inet " in line:
                    ip = line.split('inet ')[1].split(' ')[0]  # 提取IP地址部分
                    return ip
            return None  # 如果没有inet行，说明未连接
        except subprocess.CalledProcessError:
            # 如果ifconfig wlan0失败（例如接口不存在或未启用）
            return None

    def update_display(self):
        """更新OLED显示内容"""
        # 创建空白图像
        img = Image.new('1', (self.screen.width, self.screen.height), color=0)
        draw = ImageDraw.Draw(img)

        # 获取wlan0的IP地址
        ip_address = self.get_wlan0_ip()

        if ip_address:
            # 如果有IP地址，显示"IP: xxx.xxx.xxx.xxx"
            display_text = f"IP: {ip_address}"
            draw.text((3, 22), display_text, font=self.font, fill=255)
        else:
            # 如果未连接网络，显示"网络未连接"
            draw.text((3, 22), "Network not connected", font=self.font, fill=255)

        # 更新屏幕显示
        self.screen.image(img)
        self.screen.display()


def main(args=None):
    rclpy.init(args=args)
    oled_display_node = OledDisplayNode()
    rclpy.spin(oled_display_node)
    oled_display_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

图片1