・librealsenseのインストール
https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md


・ファームウェアの更新
pdfを参考にファームウェアを更新し、realsense-viewerで動作確認。
https://www.intel.com/content/dam/support/us/en/documents/emerging-technologies/intel-realsense-technology/Linux-RealSense-D400-DFU-Guide.pdf


・realsense2_cameraのテスト
realsense2_cameraをgit cloneして、roslaunch realsense2_camera rs_rgbd.launchを実行
rvizで/camera/color/image_rawが映ることを確認


・opencvとpub/subのテスト
opencv-pythonをインストールし、imageのsubscribe->opencv変換->publishがうまくいくか確認
#!/usr/bin/env python
import rospy
from cv_bridge import CvBridge
from sensor_msgs.msg import Image

class Aruco:
def __init__(self):
rospy.init_node('aruco_ros')
self.pub = rospy.Publisher('aruco_image', Image, queue_size=10)
self.cv_bridge = CvBridge()
rospy.Subscriber('/camera/color/image_raw', Image, self.callback)

def callback(self, data):
cv_image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8')
msg_image = self.cv_bridge.cv2_to_imgmsg(cv_image)
self.pub.publish(msg_image)

def main():
Aruco()
rospy.spin()
if __name__ == '__main__':
main()


・ボードの印刷
import cv2

nSquaresX = 5
nSquaresY = 6
square_length = 0.025
marker_length = 0.015

dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
board = cv2.aruco.CharucoBoard_create(
nSquaresX, nSquaresY, square_length, marker_length, dictionary
)
img = board.draw((1024, 1024))
cv2.imwrite('charuco.png', img)


・姿勢推定
#!/usr/bin/env python
import numpy as np
import rospy
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, CameraInfo

class Aruco:
def __init__(self):
# ros
rospy.init_node('aruco_ros')
self.pub = rospy.Publisher('aruco_image', Image, queue_size=10)
self.cv_bridge = CvBridge()
rospy.Subscriber('/camera/color/image_raw', Image, self.callback_image)
rospy.Subscriber('/camera/color/camera_info', CameraInfo, self.callback_camerainfo)

# aruco
nSquaresX = 5
nSquaresY = 6
self.square_length = 0.025
marker_length = 0.015
self.dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
self.board = cv2.aruco.CharucoBoard_create(
nSquaresX, nSquaresY, self.square_length, marker_length, self.dictionary
)

def callback_image(self, data):
# transform to opencv image
cv_image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8')

# detect marker
marker_corners, marker_ids, rejected_marker_corners = \
cv2.aruco.detectMarkers(cv_image, self.dictionary)
cv2.aruco.refineDetectedMarkers(
cv_image, self.board, marker_corners, marker_ids, rejected_marker_corners
)
draw_image = cv2.aruco.drawDetectedMarkers(cv_image, marker_corners, marker_ids)
# detect chessboard and estimate pose
if marker_ids is not None:
# detect chessboard
_, board_corners, board_ids = \
cv2.aruco.interpolateCornersCharuco(marker_corners, marker_ids, cv_image, self.board)
draw_image = cv2.aruco.drawDetectedCornersCharuco(draw_image, board_corners, board_ids)

# estimate pose
retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(
board_corners, board_ids, self.board, self.camera_matrix, self.dist_coeffs
)
if retval:
draw_image = cv2.aruco.drawAxis(
draw_image, self.camera_matrix, self.dist_coeffs, rvec, tvec, self.square_length
)
# transform to imgmsg
msg_image = self.cv_bridge.cv2_to_imgmsg(draw_image)
self.pub.publish(msg_image)

def callback_camerainfo(self, data):
self.camera_matrix = np.reshape(np.array(data.K), (3, 3))
self.dist_coeffs = np.array(data.D)

def main():
Aruco()
rospy.spin()
if __name__ == '__main__':
main()