ドキュメント視覚画像トピックの購読
実験の目的：
カメラを呼び出し、取得した画像情報を RViz に表示できるようになります。
機能要件：
カメラを起動して画像情報を取得します。
ハードウェアの準備：
ナビゲーションロボット * 1、HDカメラ * 1、Type-C ケーブル * 1
回路接続：
ハードウェアの実物
| カメラモジュール | Zero との接続 |
ソフトウェア設計：
設計プロセス
視覚画像トピック購読の設計
ソースコードのダウンロード
ソースコードの入手先：
資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。
ソースファイルのディレクトリ構成：
プログラムコード
プロジェクトの完全なコードはソースコードからダウンロードできます。
トピックを配信するコード：
車体側カーネル \vision_ws\src\camera_test\scripts
#!/usr/bin/env python
import rospy
import cv2
from sensor_msgs.msg import CompressedImage
from cv_bridge import CvBridge
class CameraPublisher:
def __init__(self):
rospy.init_node('continuous_camera_publisher', anonymous=True)
self.bridge = CvBridge()
#self.image_pub = rospy.Publisher('/camera/image_raw', Image, queue_size=10)
self.image_pub = rospy.Publisher('/camera/image_raw/compressed', CompressedImage, queue_size=10)
# 摄像头参数配置
self.device_id = 0 #/dev/video0
self.frame_width = 320 #160 #320 #480 #640
self.frame_height = 320 #160 #320 #320 #480
self.fps = 50 #30 #15
self.jpeg_quality = 70 #压缩质量(0~100,越高越清晰)
# 初始化摄像头
self.cap = self.initialize_camera()
def initialize_camera(self):
cap = cv2.VideoCapture(self.device_id, cv2.CAP_V4L2)
if not cap.isOpened():
rospy.logerr(f"无法打开摄像头设备 video{self.device_id}，正在重试...")
return None
# 设置摄像头参数
cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.frame_width)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frame_height)
cap.set(cv2.CAP_PROP_FPS, self.fps)
return cap
def publish_continuous_frames(self):
rate = rospy.Rate(self.fps)
retry_count = 0
max_retries = 5
while not rospy.is_shutdown():
if self.cap is None or not self.cap.isOpened():
if retry_count >= max_retries:
rospy.logerr("摄像头初始化失败，超过最大重试次数")
break
self.cap = self.initialize_camera()
retry_count += 1
rate.sleep()
continue
ret, frame = self.cap.read()
if not ret:
rospy.logwarn("帧捕获失败，尝试重新初始化摄像头")
self.cap.release()
self.cap = self.initialize_camera()
retry_count += 1
continue
frame = cv2.flip(frame, -1)
try:
# === 关键修改：压缩图像并发布 ===
# 将帧压缩为JPEG字节流
_, jpeg_data = cv2.imencode(
'.jpg',
frame,
[int(cv2.IMWRITE_JPEG_QUALITY), self.jpeg_quality]
)
# 构建 CompressedImage 消息
compressed_msg = CompressedImage()
compressed_msg.header.stamp = rospy.Time.now()
compressed_msg.format = "jpeg"
compressed_msg.data = jpeg_data.tobytes() # 直接填充压缩后的二进制数据
self.image_pub.publish(compressed_msg)
retry_count = 0
except Exception as e:
rospy.logerr(f"图像压缩或发布失败: {str(e)}")
rate.sleep()
# 清理资源
if self.cap and self.cap.isOpened():
self.cap.release()
if __name__ == '__main__':
try:
cp = CameraPublisher()
cp.publish_continuous_frames()
except rospy.ROSInterruptException:
rospy.loginfo("摄像头节点已关闭")
トピックを購読するコード
仮想マシン側カーネル \uni_robot\uni_robot\src\vision_test\scripts
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import CompressedImage, Image
from cv_bridge import CvBridge
class ImageRepublisher:
def __init__(self):
rospy.init_node('image_republisher_node', anonymous=True)
# 初始化CV Bridge
self.bridge = CvBridge()
# 订阅压缩图像话题
self.compressed_sub = rospy.Subscriber(
"/camera/image_raw/compressed",
CompressedImage,
self.compressed_callback,
queue_size=1,
buff_size=2**24 # 大缓冲区防止丢帧
)
# 发布未压缩图像话题
self.image_pub = rospy.Publisher(
"/camera/image_decompressed", # 新话题名称，供RViz订阅
Image,
queue_size=1 # 小队列降低延迟
)
rospy.loginfo("节点已启动：正在转发 /camera/image_raw/compressed → /camera/image_decompressed")
def compressed_callback(self, msg):
try:
# 将压缩数据转换为OpenCV图像
np_arr = np.frombuffer(msg.data, np.uint8)
cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
if cv_image is None:
rospy.logwarn("图像解码失败！")
return
# 转换为ROS Image消息并发布
brightened_image = cv2.convertScaleAbs(cv_image, alpha=2.5, beta=0)
ros_image = self.bridge.cv2_to_imgmsg(brightened_image, "bgr8")
ros_image.header = msg.header # 保持时间戳一致
self.image_pub.publish(ros_image)
cv2.imwrite('/home/uni_robot/src/vision_test/decompressed.jpg', cv_image)
except Exception as e:
rospy.logerr(f"图像处理异常: {str(e)}")
if __name__ == '__main__':
try:
republisher = ImageRepublisher()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("节点已关闭")
操作手順：
- リモート端末にアクセスします。ターミナルを開き、次のコマンドを入力します。
ssh uni@192.168.8.207
Enterキーを押してから、パスワード uni を入力します。下図を参考にしてください。
- 車体の画像配信プログラムをリモートで起動します。引き続きターミナルで次のコマンドを入力します。
roslaunch camera_test pub_img.launch
- ローカルの画像購読＋RViz 表示プログラムを起動します。Ctrl + Shift + T で新しいターミナルタブを開き、次のコマンドを入力します。
roslaunch vision_test sub_img.launch
実行結果は以下のとおりです。
Enterキーを押すと、取得した画像情報が RViz に表示されます。
サポート窓口
資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。