チュートリアルプログラムTurtlesimの亀をPythonで周回させてみた。
亀が画面の各辺から一定の距離を保って周回する。亀の進行を妨害しても一定の距離に回帰しようとする。
時計回りか反時計回りするかは、スタート時に亀が向いていた方向による。
動作環境は、WSL2 jazzyである。
このプログラムは、このサイトのautonomous_controller.pyを参考にさせて頂いた。
上の図は、亀が画面の右上のコーナーを下から右に回転するところである。回転を開始するときの亀の向きはほぼ1/2piであり、回転後に右を向くと角度はpiになる。亀が反時計回りに回るときは4つのコーナーでの回転をすべてこの角度の関係になるようにした。すなわち、反時計回りにコーナーを回るとは、
亀がほぼ1/2piから回転を開始し、piに等しいかpiよりも大きな角度になったら回転を止める。
逆に、時計回りにコーナーを回るとは、
亀がほぼ3/2piから回転を開始し、piに等しいかpiよりもちいさな角度になったら回転を止める。
となる。こうすれば、コーナーを回るときの角度の関係がすべて同じとなり、デバッグがしやすくなった。
使い方
WSL2上でターミナルを4つ起動し、それぞれに次のように入力する。
ros2 run turtlesim turtlesim_node # (1) Turtlesimの画面を表示する
ros2 run turtlesim turtle_teleop_key # (2) キーボードから矢印キーを使って亀を動かす
ros2 topic list -t # (3) なぜか初回、これをやりないとキー入力が有効にならない。
ros2 service call /clear std_srvs/srv/Empty # (4) 亀の航跡を消す。
ros2 run my_turtlesim go_along_wall # (5) Pythonプログラムを起動する。Pythonコード
Pythonコード
""" go_along_wall.py 2025.01.06
TurtlieSimの亀が壁に沿って周回する。`
"""
import rclpy
from rclpy.node import Node
import sys
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import pi
limit_size = 11.1 # 亀が動ける領域のx,y上限
dis_from_wall = 1.2 # 亀が通過する壁からの距離
dis_from_front_wall = dis_from_wall # 亀が向きを変える正面の壁からの距離
vel_x = 1.0 # 亀の前進速度
vel_angular = 1.0 # 亀がコーナーを曲がるときの回転速度
recovery_angular = 0.25 # 壁からの距離が離れていた時の回復用回転速度
class PublisherSubscriber(Node):
def __init__(self):
super().__init__('go_along_wall')
#self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)
self.publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
self.subscription = self.create_subscription(
Pose,
'/turtle1/pose',
self.subscription_callback,
10)
self.subscription # prevent unused variable warning
self.cmd_vel = Twist()
self.nowRotating = False
self.side_wall = "" # 沿って進む壁 E,W,S,N
self.side_wall_index = 0 # 側壁のインデックス
self.front_wall_index = 0 # 前方の壁インデックス
self.ccw = True # 時計回りならTrue
self.dis_from_side_walll = 0 # 側面からの距離
self.dis_from_front_walll = 0 # 前面からの距離
self.theta = 0 # 亀のPoseのthetaを0～pi又はpi～2piに変換した値
def subscription_callback(self, data):
global dis_from_wall, dis_from_front_wall
if self.side_wall == "":
self.get_side_wall( data)
front_dir = pi * self.front_wall_index / 2 # 正面の角度
self.get_dis_from_wall(data) # 壁からの側面、正面の距離を取得する。
# 壁に接近し回転開始するか判定する。
if self.dis_from_front_walll < dis_from_front_wall or self.nowRotating:
if self.ccw:
# 反時計回り
# self.front_wall_index == 2の状態に補正する
if self.front_wall_index == 0: #---------- +pi
front_dir += pi
self.theta = data.theta + pi
elif self.front_wall_index == 1: #-------- +1/2pi
front_dir += pi/2
self.theta = data.theta + pi/2
elif self.front_wall_index == 2: #-------- +0
self.theta = data.theta
if data.theta < 0:
self.theta = pi*2 + data.theta
elif self.front_wall_index == 3: #-------- -1/2pi
front_dir += -pi/2
if data.theta >= 0:
self.theta = data.theta - pi/2
else:
self.theta = (pi*2 + data.theta) - pi/2
# 回転終了判定
if front_dir <= self.theta:
self.cmd_vel.linear.x = 0.5
self.cmd_vel.angular.z = 0.0
self.nowRotating = False
print(f'END Rotating --- dis_from_side_walll:{self.dis_from_side_walll:.1f}, dis_from_front_walll:{self.dis_from_front_walll:.1f}, front_dir:{pi * self.front_wall_index / 2:.2f}, data.theta:{data.theta:.2f}')
self.chenge_wall() # 沿って移動する側面と前面を更新する。
print(f'index:{self.front_wall_index}, front_dir:{front_dir:.2f}, self.theta:{self.theta:.2f}, data.theta:{data.theta:.2f}')
else:
self.cmd_vel.linear.x = 0.0
self.cmd_vel.angular.z = vel_angular
self.nowRotating = True
#print('nowRotating')
else:
# 時計回り
front_dir += pi # 逆回りなので正面の角度も逆向きになる
self.theta = data.theta
# self.front_wall_index == 2の状態に補正する
if self.front_wall_index == 0: #---------- +0
if data.theta < 0:
self.theta = pi*2 + data.theta
elif self.front_wall_index == 1: #-------- -1/2pi
front_dir += -pi/2
self.theta = data.theta + 3/2*pi - 0.01
elif self.front_wall_index == 2: #-------- -pi
front_dir += -pi
self.theta = data.theta + pi
elif self.front_wall_index == 3: #-------- +1/2pi
front_dir += -3/2*pi
if data.theta < 0:
self.theta = pi*2 + data.theta
self.theta += pi/2
# 回転終了判定
if front_dir >= self.theta:
self.cmd_vel.linear.x = 0.5
self.cmd_vel.angular.z = 0.0
self.nowRotating = False
print(f'END Rotating --- dis_from_side_walll:{self.dis_from_side_walll:.1f}, dis_from_front_walll:{self.dis_from_front_walll:.1f}, front_dir:{(pi*self.front_wall_index/2+pi):.2f}, data.theta:{data.theta:.2f}')
self.chenge_wall() # 沿って移動する側面と前面を更新する。
print(f'index:{self.front_wall_index}, front_dir:{front_dir:.2f}, self.theta:{self.theta:.2f}, data.theta:{data.theta:.2f}')
else:
self.cmd_vel.linear.x = 0.0
self.cmd_vel.angular.z = -vel_angular
self.nowRotating = True
#print('cs nowRotating')
else:
# 壁に沿って前進中
#print(f'dis_from_front_walll:{self.dis_from_front_walll:.1f}, data.x:{data.x:.1f}, data.y:{data.y:.1f}')
if dis_from_wall - 0.02 <= self.dis_from_side_walll <= dis_from_wall + 0.02:
self.cmd_vel.linear.x = vel_x
self.cmd_vel.angular.z = 0.0
else:
if dis_from_wall > self.dis_from_side_walll:
if self.ccw: # 時計回り
self.cmd_vel.linear.x = 0.6
self.cmd_vel.angular.z = recovery_angular
else:
self.cmd_vel.linear.x = 0.6
self.cmd_vel.angular.z = -recovery_angular
else:
if self.ccw: # 時計回り
self.cmd_vel.linear.x = 0.6
self.cmd_vel.angular.z = -recovery_angular
else:
self.cmd_vel.linear.x = 0.6
self.cmd_vel.angular.z = recovery_angular
self.publisher.publish(self.cmd_vel) #
def get_side_wall(self, data):
# 亀に最も近い壁と前面の壁のインデックスを求める。
# 下(S):0, 右(E):1, 上(N):2, 左(W):3,
# 四方の壁からの距離 S,E,N,W
dis = [data.y-0, limit_size-data.x, limit_size-data.y, data.x-0]
min_value = min(dis)
self.side_wall_index = dis.index(min_value) # 側壁のインデックス
# 側壁のインデックスに対して時計回りの角度
# 亀のPoseのthetaを-pi～piから0～2piに変換する。
if data.theta < 0:
theta_2pi = pi*2 + data.theta
else:
theta_2pi = data.theta
# 亀が向いている角度の範囲の中心
dir = pi * self.side_wall_index / 2
if dir - pi*1/2 <= theta_2pi < dir + pi*1/2:
# 反時計回り
self.ccw = True
# 正面の壁のインデックス
self.front_wall_index = self.side_wall_index + 1
if self.front_wall_index > 3:
self.front_wall_index = 0
else:
# 時計回り
self.ccw = False
# 正面の壁のインデックス
self.front_wall_index = self.side_wall_index - 1
if self.front_wall_index < 0:
self.front_wall_index = 3
self.side_wall = ['S', 'E', 'N', 'W'][self.side_wall_index]
front_wall = ['S', 'E', 'N', 'W'][self.front_wall_index]
print(f'side_wall: {self.side_wall}, front_wall: {front_wall}, ccw:{self.ccw}')
def get_dis_from_wall(self, data):
# 亀と側面と前面の壁との距離ょ求める。
dis = [data.y-0, limit_size-data.x, limit_size-data.y, data.x-0]
self.dis_from_side_walll = dis[self.side_wall_index]
self.dis_from_front_walll = dis[self.front_wall_index]
def chenge_wall(self):
# 側面と前面の壁のインデックスを一つ進める。
if self.ccw:
# 側壁のインデックス
self.side_wall_index = self.side_wall_index + 1
if self.side_wall_index > 3:
self.side_wall_index = 0
# 正面の壁のインデックス
self.front_wall_index = self.side_wall_index + 1
if self.front_wall_index > 3:
self.front_wall_index = 0
else:
# 側壁のインデックス
self.side_wall_index = self.side_wall_index - 1
if self.side_wall_index < 0:
self.side_wall_index = 3
# 正面の壁のインデックス
self.front_wall_index = self.side_wall_index - 1
if self.front_wall_index < 0:
self.front_wall_index = 3
self.side_wall = ['S', 'E', 'N', 'W'][self.side_wall_index]
front_wall = ['S', 'E', 'N', 'W'][self.front_wall_index]
print(f'side_wall: {self.side_wall}, front_wall: {front_wall}')
def main():
rclpy.init(args=sys.argv)
pubsub = PublisherSubscriber()
rclpy.spin(pubsub)
pubsub.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()