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