dwb local plannerの実装を理解していきます。
1997年のDynamic Window Approach(DWA)という論文がProbabilistic Roboticsの著者達によって提案され、それはROS1で実装され、その後にROS2/nav2となり機能をpluginとして交換しやすくした実装をdwbと呼ぶようです。
local plannerですね。
理論の前にRVizでlocal plannerを表示させて、ヒョコヒョコというか、ピロピロというか、出てるやつを確認しておくと理解が早いと思います。
通常であれば/local_planです。RVizで[Add]で[By topic]で見つかります。おそらく濃い青い線で表示されます。
自分で考える
source code解析をする前に自分で作ることを考えるとします。
詳解確率ロボティクスのPythonをお手本として回転のみでgoalを目指す実装をします。
障害物回避は後でやります。
詳解確率ロボティクスのPythonはrobotの動作がわかりやすく拡張もやりやすいので利用させていただきます。
整理する
星はgoalを表します。
robot(中心)とgoal(中心)で線を引きrobotの向きとの差が1度未満であれば前進する仕様とします。
robot座標から見てgoalがy<-1(deg)であれば時計回りとします。
robot座標から見てgoalが1(deg)<yであれば反時計回りとします。
robot座標から見てgoalが(中心)とgoal(中心)で線を引きrobotの向きとの差が179度より大きいまたは-179度より小さい場合は時計回りの仕様とします。
output
goalに到着の条件を実装していないためgoalを目指し続けます。
実装
実装です。
dwb_local_planner_001.cpp
import matplotlib
# matplotlib.use('nbagg')
import matplotlib.animation as anm
import matplotlib.pyplot as plt
import math
from math import sin, cos, degrees
import matplotlib.patches as patches
import numpy as np
import random
import time
# ------------------------------------------------------------------------------
PI = 3.141592
DEGREE_1 = PI/180
DEGREE_179 = DEGREE_1*179
ANGULAR_VELOCITY_MAX = 0.1
class World:
def __init__( self, time_span, time_interval ):
self.objects = []
self.time_span = time_span
self.time_interval = time_interval
self.file_name_counter = 0
def append( self,obj ):
self.objects.append(obj)
def draw(self):
fig = plt.figure(figsize=(8,8)) # fig = plt.figure(figsize=(4,4))
## make axes
ax = fig.add_subplot(111)
ax.set_aspect('equal')
ax.set_xlim(-10,10)
ax.set_ylim(-10,10)
ax.set_xlabel("X",fontsize=10)
ax.set_ylabel("Y",fontsize=10)
elems = []
tmp_1 = int( self.time_span/self.time_interval ) + 1
tmp_2 = int( self.time_interval )
self.ani = anm.FuncAnimation(fig, self.one_step, fargs=(elems, ax), frames=tmp_1, interval=tmp_2, repeat=False)
plt.show()
def one_step(self, i, elems, ax):
while elems: elems.pop().remove()
time_str = "t = %.2f[s]" % (self.time_interval*i)
elems.append(ax.text(-4.4, 4.5, time_str, fontsize=10))
##
elems.append(ax.text(-4.4, 4.5, time_str, fontsize=10))
for obj in self.objects:
obj.draw(ax, elems)
if hasattr(obj, "one_step"): obj.one_step(self.time_interval)
# ------------------------------------------------------------------------------
class Map:
def __init__(self):
self.landmarks = []
self.goal = []
def append_landmark(self, landmark):
landmark.id = len(self.landmarks)
self.landmarks.append(landmark)
def append_goal( self, goal ):
self.goal.append( goal )
def draw( self, ax, elems ):
for goal in self.goal: goal.draw(ax, elems)
# ------------------------------------------------------------------------------
class Goal:
def __init__( self, x, y ):
self.pos = np.array([x, y]).T
self.id = None
def draw( self, ax, elems ):
c = ax.scatter(self.pos[0], self.pos[1], s=100, marker="*", label="goal", color="red")
elems.append(c)
elems.append(ax.text(self.pos[0], self.pos[1], "id:" + str(self.id), fontsize=10))
# ------------------------------------------------------------------------------
## x : robot position x
## y : robot position y
## yaw : robot position yaw
class Robot:
def __init__( self, pose, nu, omega, goal, color="black" ):
self.pose = pose
self.r = 0.2
self.color = color
self.poses = [pose]
self.nu = nu
self.omega = omega
self.goal = goal
self.angle = 0.0
self.diff_angle = 0.0
self.g_x_in_robot_flame = 0.0
self.g_y_in_robot_flame = 0.0
self.tmp_x = 0.0
self.tmp_y = 0.0
def draw(self, ax, elems):
x, y, yaw = self.pose
xn = x + self.r * math.cos(yaw)
yn = y + self.r * math.sin(yaw)
##
elems += ax.plot([x,xn], [y,yn], color=self.color)
## robot's orientation
ax.plot([x,xn], [y,yn], color=self.color)
c = patches.Circle(xy=(x, y), radius=self.r, fill=False, color=self.color)
elems.append(ax.add_patch(c))
elems.append(ax.text( 2, -2, f'g_x - x: {self.tmp_x:.3f}', size=15 ))
elems.append(ax.text( 2, -3, f'g_y - y: {self.tmp_y:.3f}', size=15 ))
elems.append(ax.text( 2, -4, f'g_x_in_robot_flame: {self.g_x_in_robot_flame:.3f}', size=15 ))
elems.append(ax.text( 2, -5, f'g_y_in_robot_flame: {self.g_y_in_robot_flame:.3f}', size=15 ))
elems.append(ax.text( 2, -6, f'angle: {degrees(self.angle):.3f}', size=15 ))
elems.append(ax.text( 2, -7, f'yaw: {degrees(self.pose[2]):.3f}', size=15 ))
elems.append(ax.text( 2, -8, f'diff_angle: {degrees(self.diff_angle):.3f}', size=15 ))
elems.append(ax.text( 2, -9, f'omega: {self.omega:.3f}', size=15 ))
self.poses.append(self.pose)
elems += ax.plot([e[0] for e in self.poses], [e[1] for e in self.poses], linewidth=0.5, color="black")
@classmethod
def state_transition(cls, nu, omega, time, pose):
t0 = pose[2]
if math.fabs(omega) < 1e-10:
return pose + np.array( [nu*math.cos(t0),
nu*math.sin(t0),
omega ] ) * time
else:
return pose + np.array( [nu/omega*(math.sin(t0 + omega*time) - math.sin(t0)),
nu/omega*(-math.cos(t0 + omega*time) + math.cos(t0)),
omega*time ] )
def one_step(self, time_interval):
# nu, omega = self.run()
nu, omega = self.dwa()
self.pose = self.state_transition( nu, omega, time_interval, self.pose )
def run( self ):
return self.nu, self.omega
def go_stragiht( self ):
return self.nu, self.omega
def dwa( self ):
## robot pose in world flame
x, y, yaw = self.pose
## 0(deg) <= yaw < 360(deg)
if (2*PI) <= yaw:
yaw = yaw - 2*PI
if yaw < 0:
yaw = yaw + 2*PI
self.pose[2] = yaw
## goal in world flame
goal = self.goal
g_x = goal.pos[0]
g_y = goal.pos[1]
print(f'x: {g_x},y: {g_y}')
## gradient = slope
angle = math.atan2( g_y - y, g_x - x )
## 0(deg) <= angle < 360(deg)
if angle < 0:
angle = angle + 2*PI
self.angle = angle
print(f'robot yaw: {yaw}')
print(f'angle (rad): {angle}')
print(f'angle (deg): {math.degrees(angle)}')
print(f'robot yaw - angle difference: {math.degrees(yaw - angle)}')
## goal in robot flame
tmp_x = g_x - x
tmp_y = g_y - y
g_x_in_robot_flame = tmp_x * cos(-yaw) - tmp_y * sin(-yaw)
g_y_in_robot_flame = tmp_x * sin(-yaw) + tmp_y * cos(-yaw)
## for debug
self.g_x_in_robot_flame = g_x_in_robot_flame
self.g_y_in_robot_flame = g_y_in_robot_flame
self.tmp_x = tmp_x
self.tmp_y = tmp_y
diff_angle = angle - yaw
self.diff_angle = diff_angle
## the goal is on the left
if 0 < g_y_in_robot_flame:
if abs(diff_angle) < DEGREE_1:
## move forward
change_omega = 0
else:
change_omega = abs(diff_angle) * 0.1
## the goal is on the right
if g_y_in_robot_flame < 0:
if DEGREE_179 < abs(diff_angle) and abs(diff_angle) < DEGREE_181:
## Force clockwise
change_omega = ANGULAR_VELOCITY_MAX
else:
change_omega = - abs(diff_angle) * 0.1
self.omega = change_omega
return self.nu, self.omega
# ------------------------------------------------------------------------------
if __name__ == '__main__':
## 0.01 : very slow
# world = World( 15, 0.1 )
# world = World( 120, 0.1 )
world = World( 200, 0.2 )
# world = World( 120, 0.04 )
# world = World( 120, 0.01 )
m = Map()
goal = Goal(4,8)
m.append_goal(goal)
world.append( m )
robot = Robot( np.array([0, 0, 0]).T, nu=0.2, omega=0.0, goal=goal )
# robot = Robot( np.array([0, 0, 1]).T, nu=0.2, omega=0.0, goal=goal )
# robot = Robot( np.array([0, 0, 0]).T, nu=1.0, omega=1.0, goal=goal )
world.append( robot )
world.draw()
準備。
PI = 3.141592
DEGREE_1 = PI/180
DEGREE_179 = DEGREE_1*179
ANGULAR_VELOCITY_MAX = 0.1
debug.
elems.append(ax.text( 2, -2, f'g_x - x: {self.tmp_x:.3f}', size=15 ))
elems.append(ax.text( 2, -3, f'g_y - y: {self.tmp_y:.3f}', size=15 ))
elems.append(ax.text( 2, -4, f'g_x_in_robot_flame: {self.g_x_in_robot_flame:.3f}', size=15 ))
elems.append(ax.text( 2, -5, f'g_y_in_robot_flame: {self.g_y_in_robot_flame:.3f}', size=15 ))
elems.append(ax.text( 2, -6, f'angle: {degrees(self.angle):.3f}', size=15 ))
elems.append(ax.text( 2, -7, f'yaw: {degrees(self.pose[2]):.3f}', size=15 ))
elems.append(ax.text( 2, -8, f'diff_angle: {degrees(self.diff_angle):.3f}', size=15 ))
elems.append(ax.text( 2, -9, f'omega: {self.omega:.3f}', size=15 ))
robot(中心)とgoal(中心)で線を引きworld座標系でのその傾きを計算。変数名は良くはない。
angle = math.atan2( g_y - y, g_x - x )
robot座標系でのgoalの位置。world座標系から計算。
回転の説明はこちら。
## goal in robot flame
tmp_x = g_x - x
tmp_y = g_y - y
g_x_in_robot_flame = tmp_x * cos(-yaw) - tmp_y * sin(-yaw)
g_y_in_robot_flame = tmp_x * sin(-yaw) + tmp_y * cos(-yaw)
最初に図で説明した内容を実装。0.1は動作を見やすくするために適当な係数を付けているだけです。
## the goal is on the left
if 0 < g_y_in_robot_flame:
if abs(diff_angle) < DEGREE_1:
## move forward
change_omega = 0
else:
change_omega = abs(diff_angle) * 0.1
## the goal is on the right
if g_y_in_robot_flame < 0:
if DEGREE_179 < abs(diff_angle):
## Force clockwise
change_omega = ANGULAR_VELOCITY_MAX
else:
change_omega = - abs(diff_angle) * 0.1
広告
IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト
