0. はじめに

ROS1 Kineticで実装されているArucoマーカーを用いた自動駐車システム(automatic parking vision)をROS2 Humbleで実装しました。
ROS1 Kinetic github
ROS1 Kinetic youtube
navigation2ほどの自由な経路計画はできませんが、毎回特定の位置に戻る行動をする場合にシンプルな方法で実装することができます。

1. 実行環境

項目	値
Host PC	Ubuntu 22.04
Raspberry Pi	Raspberry Pi 4 Model B
Robot	Turtlebot3 Burger
USB Camera	Buffalo BSW500MBK

2. Aruco マーカーを読み取る

2-1. カメラ映像のPublish

v4l2_cameraを用います。
インストール等を進めて実行してください。こちらの記事が参考になりました。今回はラズパイにカメラを挿すので、ラズパイ上で実行してください。
「image-transportパッケージの追加」以降は行わなくても今回は問題ありません。

ros2 run rqt_image_view rqt_image_view

でカメラ映像を取得することが確認できたらOKです。

2-2. カメラキャリブレーション

　Arucoマーカーの姿勢推定をするためにキャリブレーションファイルを作成します。こちらもラズパイ上で実行してください。
キャリブレーションからArucoマーカーの認識までは、こちらの記事が参考になりました。
　チェッカーボードはこちらから自作できます。
以下のパラメーターで作成したサンプル画像です。

Target Type: Checkerboard
Board Width 210 [mm]
Board Height 297 [mm]
Rows 8
Columns 10
Checker Width 20 [mm]

　キャリブレーション実行コマンドの--sizeオプションについて、キャリブレーションはチェッカーボードの内部の頂点を使用するので、四角の数が「8x10」のボードは内部の頂点パラメータ「7x9」を使用することに注意してください。これに気づかず時間を溶かしてしまいました...。

ros2 run camera_calibration cameracalibrator --size 7x9 --square 0.02 --ros-args -r image:=/my_camera/image_raw -p camera:=/my_camera

ArucoマーカーをROS2で使用する

　ホームディレクトリからautomatic_parking_visionという名前でワークスペースを作成し、srcに移動します

mkdir -p automatic_parking_vision_ws/src
cd automatic_parking_vision_ws/src

　Arucoマーカーの位置と向き、及びIDの情報を検出するためにJMU-ROBOTICS-VIVAさんの作成したros2_arucoパッケージを使わせていただきます。

git clone https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco.git -b humble

この時、 aruco_node.pyを以下のように変更しています。コピー＆ペースト等でこちらを使用してください。
主な変更箇所

複数のマーカー情報をトピックに流していたが、変更後は各フレームの初めに検出されたマーカー情報のみをトピックに流すように変更しています
これは後述するautomatic_parking_visionが単一のマーカー情報をサブスクライブするようになっているためです。

aruco_node.py修正版

aruco_node.py

import rclpy
import rclpy.node
from rclpy.qos import qos_profile_sensor_data
from cv_bridge import CvBridge
import numpy as np
import cv2
import tf_transformations
from sensor_msgs.msg import CameraInfo
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseArray, Pose
# from ros2_aruco_interfaces.msg import ArucoMarkers
from visualization_msgs.msg import Marker
from rcl_interfaces.msg import ParameterDescriptor, ParameterType

from tf_transformations import euler_from_quaternion


class ArucoNode(rclpy.node.Node):
    def __init__(self):
        super().__init__("aruco_node")

        # Declare and read parameters
        self.declare_parameter(
            name="marker_size",
            value=0.0625,
            descriptor=ParameterDescriptor(
                type=ParameterType.PARAMETER_DOUBLE,
                description="Size of the markers in meters.",
            ),
        )

        self.declare_parameter(
            name="aruco_dictionary_id",
            value="DICT_5X5_250",
            descriptor=ParameterDescriptor(
                type=ParameterType.PARAMETER_STRING,
                description="Dictionary that was used to generate markers.",
            ),
        )

        self.declare_parameter(
            name="image_topic",
            value="/image_raw",
            descriptor=ParameterDescriptor(
                type=ParameterType.PARAMETER_STRING,
                description="Image topic to subscribe to.",
            ),
        )

        self.declare_parameter(
            name="camera_info_topic",
            value="/camera_info",
            descriptor=ParameterDescriptor(
                type=ParameterType.PARAMETER_STRING,
                description="Camera info topic to subscribe to.",
            ),
        )

        self.declare_parameter(
            name="camera_frame",
            value="my_camera",
            descriptor=ParameterDescriptor(
                type=ParameterType.PARAMETER_STRING,
                description="Camera optical frame to use.",
            ),
        )

        self.marker_size = (
            self.get_parameter("marker_size").get_parameter_value().double_value
        )
        self.get_logger().info(f"Marker size: {self.marker_size}")

        dictionary_id_name = (
            self.get_parameter("aruco_dictionary_id").get_parameter_value().string_value
        )
        self.get_logger().info(f"Marker type: {dictionary_id_name}")

        image_topic = (
            self.get_parameter("image_topic").get_parameter_value().string_value
        )
        self.get_logger().info(f"Image topic: {image_topic}")

        info_topic = (
            self.get_parameter("camera_info_topic").get_parameter_value().string_value
        )
        self.get_logger().info(f"Image info topic: {info_topic}")

        self.camera_frame = (
            self.get_parameter("camera_frame").get_parameter_value().string_value
        )
        self.get_logger().info(f"Camera frame: {self.camera_frame}")

        # Make sure we have a valid dictionary id:
        try:
            dictionary_id = cv2.aruco.__getattribute__(dictionary_id_name)
            if type(dictionary_id) != type(cv2.aruco.DICT_5X5_100):
                raise AttributeError
        except AttributeError:
            self.get_logger().error(
                "bad aruco_dictionary_id: {}".format(dictionary_id_name)
            )
            options = "\n".join([s for s in dir(cv2.aruco) if s.startswith("DICT")])
            self.get_logger().error("valid options: {}".format(options))

        # Set up subscriptions
        self.info_sub = self.create_subscription(
            CameraInfo, info_topic, self.info_callback, qos_profile_sensor_data
        )

        self.create_subscription(
            Image, image_topic, self.image_callback, qos_profile_sensor_data
        )

        # Set up publishers
        self.poses_pub = self.create_publisher(PoseArray, "aruco_poses", 10)
        self.markers_pub = self.create_publisher(Marker, "aruco_marker", 10)
        # self.markers_pub = self.create_publisher(Int64[], "aruco_markers", 10)

        # Set up fields for camera parameters
        self.info_msg = None
        self.intrinsic_mat = None
        self.distortion = None

        self.aruco_dictionary = cv2.aruco.Dictionary_get(dictionary_id)
        #self.aruco_parameters = cv2.aruco.Detectmarker_idsorParameters_create()
        self.aruco_parameters = cv2.aruco.DetectorParameters_create()
        self.bridge = CvBridge()
        self.get_logger().info("finish init")


    def info_callback(self, info_msg):
        self.info_msg = info_msg
        self.intrinsic_mat = np.reshape(np.array(self.info_msg.k), (3, 3))
        self.distortion = np.array(self.info_msg.d)
        # Assume that camera parameters will remain the same...
        self.destroy_subscription(self.info_sub)

    def image_callback(self, img_msg):
        if self.info_msg is None:
            self.get_logger().warn("No camera info has been received!")
            return
        cv_image = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding="bgr8") #mono8
        marker = Marker()
        pose_array = PoseArray()
        if self.camera_frame == "":
            marker.header.frame_id = self.info_msg.header.frame_id
            pose_array.header.frame_id = self.info_msg.header.frame_id
        else:
            marker.header.frame_id = self.camera_frame
            pose_array.header.frame_id = self.camera_frame

        marker.header.stamp = img_msg.header.stamp
        pose_array.header.stamp = img_msg.header.stamp

        corners, marker_ids, rejected = cv2.aruco.detectMarkers(
            cv_image, self.aruco_dictionary, parameters=self.aruco_parameters
        )
        if marker_ids is not None:
            cv2.aruco.drawDetectedMarkers(cv_image, corners, marker_ids)

            if cv2.__version__ > "4.0.0":
                rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                    corners, self.marker_size, self.intrinsic_mat, self.distortion
                )
                

            else:
                rvecs, tvecs = cv2.aruco.estimatePoseSingleMarkers(
                    corners, self.marker_size, self.intrinsic_mat, self.distortion
                )
            

            pose = Pose()
            pose.position.x = tvecs[0][0][0]
            pose.position.y = tvecs[0][0][1]
            pose.position.z = tvecs[0][0][2]

            rot_matrix = np.eye(4)
            rot_matrix[0:3, 0:3] = cv2.Rodrigues(np.array(rvecs[0][0]))[0]
            quat = tf_transformations.quaternion_from_matrix(rot_matrix)

            pose.orientation.x = quat[0]
            pose.orientation.y = quat[1]
            pose.orientation.z = quat[2]
            pose.orientation.w = quat[3]

            marker.pose=pose
            marker.id=int(marker_ids[0][0])

            quaternion = (
                pose.orientation.x,
                pose.orientation.y,
                pose.orientation.z,
                pose.orientation.w)
            theta = euler_from_quaternion(quaternion)[1] #xz軸なのでpitch

            self.poses_pub.publish(pose_array)
            self.markers_pub.publish(marker)

        #xyz軸の表示
        for i, corner in enumerate(corners):
            rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                    corner, self.marker_size, self.intrinsic_mat, self.distortion
                )
            
            rvec = np.squeeze(rvecs)
            tvec = np.squeeze(tvecs)
            draw_pole_length = self.marker_size / 2
            cv2.aruco.drawAxis(cv_image, self.intrinsic_mat, self.distortion, rvec, tvec, draw_pole_length)

        cv2.imshow("camera", cv_image)
        cv2.waitKey(1)


def main():
    rclpy.init()
    node = ArucoNode()
    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

　検出に用いるArucoマーカーは、

ros2 run ros2_aruco aruco_generate_marker --id 1

を実行することで生成が可能です。最後に指定するidによって異なるマーカーが生成されます。idは0~249まで指定することができます。

ファイルの変更を保存したらビルドします。

cd ~/automatic_parking_vision_ws
colcon build

別のターミナルを開いて、実行します。

cd ~/automatic_parking_vision_ws
. install/setup.bash
ros2 run ros2_aruco aruco_node

最後に

ros2 topic echo /aruco_marker

を実行してpositionとorientationの情報が確認できたら成功です。

3. automatic_parking_vision

ワークスペースに移動して新しくパッケージを作成します。

cd ~/automatic_parking_vision_ws/src
ros2 pkg create --build-type ament_python automatic_parking_vision

/automatic_parking_vision/automatic_parking_visionにmain.pyを作成し、中身を書きます。

　今回参考にしたROS1のgithubから、'''turtlebot3_applications/turtlebot3_automatic_parking_vision/nodes
/automatic_parking_vision'''のファイルをROS2対応するように書き換えたのが以下のコードです。コピー＆ペースト等で使用してください。
主な変更箇所

マーカーを立て掛けた時、地面に水平なマーカーの軸はxz軸になるため、fnGet2DMarkerPoseにおいてマーカーのpositionにはposition.xとposition.zを取得しており、thetaはpitch値を取得しています
fnSeqParkingは、カメラの中心にマーカーが来るように回転→遠ければ直進→...を繰り返しています
急発進・急停止により機体が揺れてしまうので、全体的にlinearとangularの速度を落としています

automatic_parking_vision/main.py修正版

main.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

from enum import Enum
import math

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from visualization_msgs.msg import Marker

from tf_transformations import euler_from_quaternion
import numpy as np


MARKER_ID_DETECTION = 124 # id to be recognized


class AutomaticParkingVision(Node):
    def __init__(self):
        super().__init__('automatic_parking_vision')

        self.sub_odom_robot = self.create_subscription(
            Odometry,
            '/odom',
            self.cbGetRobotOdom,
            10)

        self.sub_info_marker = self.create_subscription(
            Marker,
            '/aruco_marker',
            self.cbGetMarkerOdom,
            20)

        self.pub_cmd_vel = self.create_publisher(
            Twist,
            '/cmd_vel',
            10)

        self.ParkingSequence = Enum(
            'ParkingSequence',
            'searching_parking_lot changing_direction moving_nearby_parking_lot parking stop finished')

        self.NearbySequence = Enum(
            'NearbySequence',
            'initial_turn go_straight turn_right parking')

        self.current_nearby_sequence = self.NearbySequence.initial_turn.value
        self.current_parking_sequence = self.ParkingSequence.searching_parking_lot.value

        self.robot_2d_pose_x = .0
        self.robot_2d_pose_y = .0
        self.robot_2d_theta = .0
        self.marker_2d_pose_x = .0
        self.marker_2d_pose_z = .0
        self.marker_2d_theta = .0

        self.previous_robot_2d_theta = .0
        self.total_robot_2d_theta = .0
        self.is_triggered = False

        self.is_sequence_finished = False

        self.is_odom_received = False
        self.is_marker_pose_received = False

        self.timer = self.create_timer(0.1, self._run)

        #環境に応じて値を調整
        self.arctanAtCenter = 1.5
        self.angleTurnThreshold = 0.05
        self.nearbyDistanceThreshold = 0.15


    def _run(self):
        if self.is_odom_received is True:
            self.fnParking()

    def cbGetRobotOdom(self, robot_odom_msg):
        if self.is_odom_received == False:
            self.is_odom_received = True

        pos_x, pos_y, theta = self.fnGet2DRobotPose(robot_odom_msg)

        self.robot_2d_pose_x = pos_x
        self.robot_2d_pose_y = pos_y
        self.robot_2d_theta = theta

        if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.:
            d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) - 2 * math.pi
        elif (self.robot_2d_theta - self.previous_robot_2d_theta) < -5.:
            d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) + 2 * math.pi
        else:
            d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta)

        self.total_robot_2d_theta = self.total_robot_2d_theta + d_theta
        self.previous_robot_2d_theta = self.robot_2d_theta

        self.robot_2d_theta = self.total_robot_2d_theta

    def cbGetMarkerOdom(self, markers_odom_msg):
        if self.is_marker_pose_received == False:
            if markers_odom_msg.id == MARKER_ID_DETECTION:
                self.is_marker_pose_received = True

        pos_x, pos_z, theta = self.fnGet2DMarkerPose(markers_odom_msg.pose)

        self.marker_2d_pose_x = pos_x
        self.marker_2d_pose_z = pos_z
        self.marker_2d_theta = theta

    def fnParking(self):
        if self.current_parking_sequence == self.ParkingSequence.searching_parking_lot.value:
            self.get_logger().info("Start 1")
            self.is_sequence_finished = self.fnSeqSearchingGoal()

            if self.is_sequence_finished == True:
                self.get_logger().info("Finished 1")
                self.current_parking_sequence = self.ParkingSequence.changing_direction.value
                self.is_sequence_finished = False

        elif self.current_parking_sequence == self.ParkingSequence.changing_direction.value:
            self.get_logger().info("Start 2")
            self.is_sequence_finished = self.fnSeqChangingDirection()

            if self.is_sequence_finished == True:
                self.get_logger().info("Finished 2")
                self.current_parking_sequence = self.ParkingSequence.moving_nearby_parking_lot.value
                self.is_sequence_finished = False

        elif self.current_parking_sequence == self.ParkingSequence.moving_nearby_parking_lot.value:
            self.get_logger().info("Start 3")
            self.is_sequence_finished = self.fnSeqMovingNearbyParkingLot()

            if self.is_sequence_finished == True:
                self.get_logger().info("Finished 3")
                self.current_parking_sequence = self.ParkingSequence.parking.value
                self.is_sequence_finished = False

        if self.current_parking_sequence == self.ParkingSequence.parking.value:
            self.get_logger().info("Start 4")
            self.is_sequence_finished = self.fnSeqParking()

            if self.is_sequence_finished == True:
                self.get_logger().info("Finished 4")
                self.current_parking_sequence = self.ParkingSequence.stop.value
                self.is_sequence_finished = False

        elif self.current_parking_sequence == self.ParkingSequence.stop.value:
            self.get_logger().info("Start 5")
            self.fnStop()
            self.get_logger().info("Finished 5")
            self.current_parking_sequence = self.ParkingSequence.finished.value
            self.fnShutDown()
            rclpy.shutdown()

    def fnSeqSearchingGoal(self): #Search marker(turn)
        if self.is_marker_pose_received is False:
            self.desired_angle_turn = -0.6
            self.fnTurn(self.desired_angle_turn)
        else:
            self.fnStop()
            return True

    def fnSeqChangingDirection(self): #Rotate camera for marker to be in the middle of the angle of view
        desired_angle_turn = -1 * (math.atan2(self.marker_2d_pose_z - 0, self.marker_2d_pose_x - 0) - self.arctanAtCenter)
        self.fnTurn(desired_angle_turn)

        if abs(desired_angle_turn) < self.angleTurnThreshold:
            self.fnStop()
            return True
        else:
            return False

    def fnSeqMovingNearbyParkingLot(self):
        if self.current_nearby_sequence == self.NearbySequence.initial_turn.value: #turn pi/2
            self.get_logger().info("Start 3-1")
            if self.is_triggered == False:
                self.is_triggered = True
                self.initial_robot_pose_theta = self.robot_2d_theta
                self.initial_robot_pose_x = self.robot_2d_pose_x
                self.initial_robot_pose_y = self.robot_2d_pose_y
                self.initial_marker_pose_theta = self.marker_2d_theta
                self.initial_marker_pose_x = self.marker_2d_pose_x
                self.initial_marker_pose_z = self.marker_2d_pose_z

            if self.initial_marker_pose_theta < 0.0:
                angle_turn_from_start = self.robot_2d_theta - self.initial_robot_pose_theta
                desired_angle_turn = -(math.pi / 2.0) - self.initial_marker_pose_theta
                remained_angle_turn = desired_angle_turn - angle_turn_from_start
            elif self.initial_marker_pose_theta > 0.0:
                angle_turn_from_start = self.robot_2d_theta - self.initial_robot_pose_theta
                desired_angle_turn = (math.pi / 2.0) - self.initial_marker_pose_theta
                remained_angle_turn = desired_angle_turn - angle_turn_from_start
            remained_angle_turn = -1. * remained_angle_turn

            self.fnTurn(remained_angle_turn)

            if abs(remained_angle_turn) < self.angleTurnThreshold:
                self.fnStop()
                self.current_nearby_sequence = self.NearbySequence.go_straight.value
                self.is_triggered = False

        elif self.current_nearby_sequence == self.NearbySequence.go_straight.value: #go straight
            self.get_logger().info("Start 3-2")
            dist_from_start = self.fnCalcDistPoints(self.initial_robot_pose_x, self.robot_2d_pose_x, self.initial_robot_pose_y, self.robot_2d_pose_y)
            desired_dist = abs(self.initial_marker_pose_z) * abs(math.cos((math.pi / 2.) - self.initial_marker_pose_theta))
            remained_dist = desired_dist - dist_from_start

            self.fnGoStraight()
            if remained_dist < 0.01:
                self.fnStop()
                self.current_nearby_sequence = self.NearbySequence.turn_right.value

        elif self.current_nearby_sequence == self.NearbySequence.turn_right.value: # turn pi/2 in the opposite direction
            self.get_logger().info("Start 3-3")
            if self.is_triggered == False:
                self.is_triggered = True
                self.initial_robot_pose_theta = self.robot_2d_theta

            if self.initial_marker_pose_theta < 0.0:
                angle_turn_from_start = self.robot_2d_theta - self.initial_robot_pose_theta
                desired_angle_turn = (math.pi / 2.0)
                remained_angle_turn = desired_angle_turn - angle_turn_from_start
            elif self.initial_marker_pose_theta > 0.0:
                angle_turn_from_start = self.robot_2d_theta - self.initial_robot_pose_theta
                desired_angle_turn = -(math.pi / 2.0)
                remained_angle_turn = desired_angle_turn - angle_turn_from_start
            remained_angle_turn = -1. * remained_angle_turn

            self.fnTurn(remained_angle_turn)

            if abs(remained_angle_turn) < self.angleTurnThreshold:
                self.fnStop()
                self.current_nearby_sequence = self.NearbySequence.parking.value
                self.is_triggered = False
                return True

        return False

    def fnSeqParking(self): # turn and go straight
        desired_angle_turn = -1 * (math.atan2(self.marker_2d_pose_z - 0, self.marker_2d_pose_x - 0) - self.arctanAtCenter)
        if abs(desired_angle_turn) > self.angleTurnThreshold:
            self.fnTurn(desired_angle_turn)
            turn_flag = False
        else:
            turn_flag = True

        if abs(self.marker_2d_pose_z) > self.nearbyDistanceThreshold and turn_flag == True :
            self.fnGoStraight()
            straight_flag = False
        else:
            straight_flag = True

        if turn_flag and straight_flag:
            self.fnStop()
            return True
        else:
            return False

    def fnStop(self):
        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0
        self.pub_cmd_vel.publish(twist)

    def fnTurn(self, theta):
        Kp = 0.4
        angular_z = Kp * theta

        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = -angular_z
        self.pub_cmd_vel.publish(twist)

    def fnGoStraight(self):
        twist = Twist()
        twist.linear.x = 0.05
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0
        self.pub_cmd_vel.publish(twist)

    def fnTrackMarker(self, theta):
        Kp = 0.4

        angular_z = Kp * theta

        twist = Twist()
        twist.linear.x = 0.05
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = -angular_z
        self.pub_cmd_vel.publish(twist)

    def fnGet2DRobotPose(self, robot_odom_msg):
        quaternion = (
            robot_odom_msg.pose.pose.orientation.x,
            robot_odom_msg.pose.pose.orientation.y,
            robot_odom_msg.pose.pose.orientation.z,
            robot_odom_msg.pose.pose.orientation.w)
        theta = euler_from_quaternion(quaternion)[2]

        if theta < 0.0:
            theta = theta + np.pi * 2
        if theta > np.pi * 2:
            theta = theta - np.pi * 2

        pos_x = robot_odom_msg.pose.pose.position.x
        pos_y = robot_odom_msg.pose.pose.position.y

        return pos_x, pos_y, theta

    def fnGet2DMarkerPose(self, marker_odom_msg):
        quaternion = (
            marker_odom_msg.orientation.x,
            marker_odom_msg.orientation.y,
            marker_odom_msg.orientation.z,
            marker_odom_msg.orientation.w)
        theta = euler_from_quaternion(quaternion)[1] #pitch. because y-axis of the marker is upward.
        # theta_deg = math.degrees(theta)

        pos_x = marker_odom_msg.position.x
        pos_z = marker_odom_msg.position.z

        return pos_x, pos_z, theta

    def fnCalcDistPoints(self, x1, x2, y1, y2):
        return math.sqrt((x1 - x2) ** 2. + (y1 - y2) ** 2.)

    def fnShutDown(self):
        self.get_logger().info("Shutting down. cmd_vel will be 0")
        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0
        self.pub_cmd_vel.publish(twist)


def main(args=None):
    rclpy.init(args=args)
    node = AutomaticParkingVision()

    rclpy.spin(node)

    node.fnShutDown()
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

setup.pyのentry pointsに以下を追加します。

entry_points={
        'console_scripts': [
            'automatic_parking_vision = automatic_parking_vision.main:main',
        ],
    },

ファイルの変更を保存したらビルドします。

cd ~/automatic_parking_vision_ws
colcon build

別のターミナルを開いて、実行できます。

cd ~/automatic_parking_vision_ws
. install/setup.bash
ros2 run automatic_parking_vision automatic_parking_vision

4. 実行

カメラ映像のPublish
ロボットのブリングアップ(ros2 launch turtlebot3_bringup robot.launch.py)
ros2_aruco
automatic_parking_vision
　これらをビルドして実行すると、Arucoマーカーを目指した駐車がスタートします。具体的な仕様に関しては、次項で説明します。

動作動画

5. arucoマーカー認識から駐車までの流れ

　automatic_parking_visionの処理は、主に５段階に分けることができます。先述したコードでは"Start1"などとログを出して管理していましたが、詳細は以下の通りです。

1: Arucoマーカーの探索

　カメラがマーカーを捕らえるまで、機体が反時計回りに回転し続けます。

main.py

    def fnSeqSearchingGoal(self): #Search marker(turn)
        if self.is_marker_pose_received is False:
            self.desired_angle_turn = -0.6
            self.fnTurn(self.desired_angle_turn)
        else:
            self.fnStop()
            return True

2: マーカーの精密な位置を検出する

　より正確なマーカーの位置情報を手に入れるため、カメラ視野の中心にマーカーが映るように回転します。この回転はP制御で、中心に近づくにつれ速度が低下していきます。

main.py

    def fnSeqChangingDirection(self): #Rotate camera for marker to be in the middle of the angle of view
        desired_angle_turn = -1 * (math.atan2(self.marker_2d_pose_z - 0, self.marker_2d_pose_x - 0) - self.arctanAtCenter)
        self.fnTurn(desired_angle_turn)

        if abs(desired_angle_turn) < self.angleTurnThreshold:
            self.fnStop()
            return True
        else:
            return False

３: マーカーの直線上まで進行する

　マーカーから見て正面の位置に機体が来るように、回転と直進を行います。
　まずは、現在の機体とマーカーの座標とθの情報をinititalから始まる変数に格納。self.initial_marker_pose_thetaの正負でマーカーに対し機体が左右どちらにあるかを判別し、それぞれマーカー方面に90度回転します。

main.py

if self.current_nearby_sequence == self.NearbySequence.initial_turn.value: #turn pi/2
            self.get_logger().info("Start 3-1")
            if self.is_triggered == False:
                self.is_triggered = True
                self.initial_robot_pose_theta = self.robot_2d_theta
                self.initial_robot_pose_x = self.robot_2d_pose_x
                self.initial_robot_pose_y = self.robot_2d_pose_y
                self.initial_marker_pose_theta = self.marker_2d_theta
                self.initial_marker_pose_x = self.marker_2d_pose_x
                self.initial_marker_pose_z = self.marker_2d_pose_z

            if self.initial_marker_pose_theta < 0.0:
                angle_turn_from_start = self.robot_2d_theta - self.initial_robot_pose_theta
                desired_angle_turn = -(math.pi / 2.0) - self.initial_marker_pose_theta
                remained_angle_turn = desired_angle_turn - angle_turn_from_start
            elif self.initial_marker_pose_theta > 0.0:
                angle_turn_from_start = self.robot_2d_theta - self.initial_robot_pose_theta
                desired_angle_turn = (math.pi / 2.0) - self.initial_marker_pose_theta
                remained_angle_turn = desired_angle_turn - angle_turn_from_start
            remained_angle_turn = -1. * remained_angle_turn

            self.fnTurn(remained_angle_turn)

            if abs(remained_angle_turn) < self.angleTurnThreshold:
                self.fnStop()
                self.current_nearby_sequence = self.NearbySequence.go_straight.value
                self.is_triggered = False

　次に、マーカーの直線上に来るまで機体を前進させます。目的地となるdesired_distはマーカーの位置情報を基に、三角関数で導出しています。

main.py

elif self.current_nearby_sequence == self.NearbySequence.go_straight.value: #go straight
           self.get_logger().info("Start 3-2")
           dist_from_start = self.fnCalcDistPoints(self.initial_robot_pose_x, self.robot_2d_pose_x, self.initial_robot_pose_y, self.robot_2d_pose_y)
           desired_dist = abs(self.initial_marker_pose_z) * abs(math.cos((math.pi / 2.) - self.initial_marker_pose_theta))
           remained_dist = desired_dist - dist_from_start

           self.fnGoStraight()
           if remained_dist < 0.01:
               self.fnStop()
               self.current_nearby_sequence = self.NearbySequence.turn_right.value

　最後に、3の初めで実行した回転の逆に、再度90度回ることでマーカーの正面を向きます。

main.py

elif self.current_nearby_sequence == self.NearbySequence.turn_right.value: # turn pi/2 in the opposite direction
            self.get_logger().info("Start 3-3")
            if self.is_triggered == False:
                self.is_triggered = True
                self.initial_robot_pose_theta = self.robot_2d_theta

            if self.initial_marker_pose_theta < 0.0:
                angle_turn_from_start = self.robot_2d_theta - self.initial_robot_pose_theta
                desired_angle_turn = (math.pi / 2.0)
                remained_angle_turn = desired_angle_turn - angle_turn_from_start
            elif self.initial_marker_pose_theta > 0.0:
                angle_turn_from_start = self.robot_2d_theta - self.initial_robot_pose_theta
                desired_angle_turn = -(math.pi / 2.0)
                remained_angle_turn = desired_angle_turn - angle_turn_from_start
            remained_angle_turn = -1. * remained_angle_turn

            self.fnTurn(remained_angle_turn)

            if abs(remained_angle_turn) < self.angleTurnThreshold:
                self.fnStop()
                self.current_nearby_sequence = self.NearbySequence.parking.value
                self.is_triggered = False
                return True

        return False

4: 直進と角度の微調整

　ここまで無事に進んでいれば、機体はマーカーの正面を向いて後は直進するだけです。しかし、このままでは角度が少しでもずれていた場合に、マーカーに対して垂直に停車することができません。
　そのため、fnGoStraight()で直進すると同時にfnTurn()を実行することで、進みながらの角度の微調整を行っています。角度も距離も十分な場所に到着した場合のみ、fnStop()が実行されて停車します。

main.py

def fnSeqParking(self): # turn and go straight
        desired_angle_turn = -1 * (math.atan2(self.marker_2d_pose_z - 0, self.marker_2d_pose_x - 0) - self.arctanAtCenter)
        if abs(desired_angle_turn) > self.angleTurnThreshold:
            self.fnTurn(desired_angle_turn)
            turn_flag = False
        else:
            turn_flag = True

        if abs(self.marker_2d_pose_z) > self.nearbyDistanceThreshold and turn_flag == True :
            self.fnGoStraight()
            straight_flag = False
        else:
            straight_flag = True

        if turn_flag and straight_flag:
            self.fnStop()
            return True
        else:
            return False

5: 停車、及びシャットダウン

　ここまで来れば停車のプロセスはほぼ完了していますが、改めてfn.Stop() fn.fnShutDown()を実行しTwist()で使われていた変数をリセットすることで処理を終了させました。

main.py

def fnShutDown(self):
        self.get_logger().info("Shutting down. cmd_vel will be 0")
        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0
        self.pub_cmd_vel.publish(twist)

　以上が、automatic_parking_visionで行われた処理になります。

6. 修正箇所・注意点

　ROS1からROS2に移行する、という仕様変更の観点で困ったことは存外少なく、元のコードの形状を保ったままROS2で実行することができました。
　他で詰まった部分や注意点については以下の通りです。

Arucoマーカーの軸
　当初、元のコードのままでビルドすると意図しない方向に回転し続ける問題に直面していました。これは、Arucoマーカーのx,y,z軸がマーカーを立てかけたことで回転し、用いる変数が違っていたことが原因です。
記事のコードではArucoマーカーを印刷し、向きを変えずに立てかけて使えるようにしています。ただ、もし明らかに逆側を向く挙動を見せた場合はマーカーの座標情報をros2 topic echo /aruco_posesやget_logger().info()で出力し、向きを確かめるのも一手です。
通信回線の重さ
　映像を扱う特性故か、ROSによる通信を複数人が使っている環境で実行するとマーカーの認識にラグが生じて、2~3段階目から進行しなくなる状況に陥りました。
　ros2 topic echo /image_raw等で映像を受け取れているか、確認しながらの実行を推奨します。
複数のマーカーが存在する場合
　ros2_aruco自体は複数のマーカーを認識できる仕様ですが、parkingできる場所が可変であると行ったり来たり混乱を引き起こします。
　そのため、automatic_parking_visionの冒頭でMARKER_ID_DETECTION = 124(任意の数字)と記述することで、1種類のマーカーにのみ反応するようにしてあります。

7. 結論・今後

ロボットの背面にマーカーを貼り付けてturtlebot3_followerのようなことができそうですね。
turtlebot3_followerの場合、付近に障害物がある場合、障害物を進む方向として誤認識する可能性があるが、マーカーであればついていく対象を誤認することがないことがメリットとして考えられます。

参考

v4l2 camera: https://index.ros.org/r/v4l2_camera/
カメラキャリブレーション: https://navigation.ros.org/tutorials/docs/camera_calibration.html
opencv aruco : https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html
ros2 aruco : https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco/tree/humble